1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Copyright (C) 2011 Intel Corporation. All rights reserved. 4 * Copyright (C) 2014 Marvell International Ltd. 5 */ 6 7 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__ 8 9 #include <linux/init.h> 10 #include <linux/kernel.h> 11 #include <linux/list.h> 12 #include <linux/nfc.h> 13 14 #include "nfc.h" 15 #include "llcp.h" 16 17 static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; 18 19 static LIST_HEAD(llcp_devices); 20 /* Protects llcp_devices list */ 21 static DEFINE_SPINLOCK(llcp_devices_lock); 22 23 static struct workqueue_struct *llcp_wq; 24 25 static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb); 26 27 void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk) 28 { 29 write_lock(&l->lock); 30 sk_add_node(sk, &l->head); 31 write_unlock(&l->lock); 32 } 33 34 void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk) 35 { 36 write_lock(&l->lock); 37 sk_del_node_init(sk); 38 write_unlock(&l->lock); 39 } 40 41 void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock) 42 { 43 sock->remote_rw = LLCP_DEFAULT_RW; 44 sock->remote_miu = LLCP_MAX_MIU + 1; 45 } 46 47 static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock) 48 { 49 struct nfc_llcp_local *local = sock->local; 50 struct sk_buff *s, *tmp; 51 52 skb_queue_purge(&sock->tx_queue); 53 skb_queue_purge(&sock->tx_pending_queue); 54 55 if (local == NULL) 56 return; 57 58 /* Search for local pending SKBs that are related to this socket */ 59 skb_queue_walk_safe(&local->tx_queue, s, tmp) { 60 if (s->sk != &sock->sk) 61 continue; 62 63 skb_unlink(s, &local->tx_queue); 64 kfree_skb(s); 65 } 66 } 67 68 static struct sock *nfc_llcp_sock_list_pop(struct llcp_sock_list *l) 69 { 70 struct sock *sk; 71 72 write_lock(&l->lock); 73 sk = sk_head(&l->head); 74 if (sk) { 75 sock_hold(sk); 76 sk_del_node_init(sk); 77 } 78 write_unlock(&l->lock); 79 80 return sk; 81 } 82 83 static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device, 84 int err) 85 { 86 struct sock *sk; 87 struct nfc_llcp_sock *llcp_sock; 88 89 skb_queue_purge(&local->tx_queue); 90 91 while ((sk = nfc_llcp_sock_list_pop(&local->sockets))) { 92 llcp_sock = nfc_llcp_sock(sk); 93 94 lock_sock(sk); 95 96 nfc_llcp_socket_purge(llcp_sock); 97 98 if (sk->sk_state == LLCP_CONNECTED) 99 nfc_put_device(llcp_sock->dev); 100 101 if (sk->sk_state == LLCP_LISTEN) { 102 struct nfc_llcp_sock *lsk, *n; 103 struct sock *accept_sk; 104 105 list_for_each_entry_safe(lsk, n, 106 &llcp_sock->accept_queue, 107 accept_queue) { 108 bool put_creation = false; 109 110 accept_sk = &lsk->sk; 111 lock_sock_nested(accept_sk, 112 SINGLE_DEPTH_NESTING); 113 114 if (nfc_llcp_sock(accept_sk)->parent == sk) { 115 nfc_llcp_accept_unlink(accept_sk); 116 nfc_llcp_sock_unlink(&local->sockets, accept_sk); 117 118 if (err) 119 accept_sk->sk_err = err; 120 accept_sk->sk_state = LLCP_CLOSED; 121 accept_sk->sk_state_change(accept_sk); 122 sock_orphan(accept_sk); 123 put_creation = true; 124 } 125 126 release_sock(accept_sk); 127 if (put_creation) 128 sock_put(accept_sk); /* creation ref */ 129 } 130 } 131 132 if (err) 133 sk->sk_err = err; 134 sk->sk_state = LLCP_CLOSED; 135 sk->sk_state_change(sk); 136 137 release_sock(sk); 138 sock_put(sk); 139 } 140 141 /* If we still have a device, we keep the RAW sockets alive */ 142 if (device == true) 143 return; 144 145 while ((sk = nfc_llcp_sock_list_pop(&local->raw_sockets))) { 146 llcp_sock = nfc_llcp_sock(sk); 147 148 lock_sock(sk); 149 150 nfc_llcp_socket_purge(llcp_sock); 151 152 if (err) 153 sk->sk_err = err; 154 sk->sk_state = LLCP_CLOSED; 155 sk->sk_state_change(sk); 156 157 release_sock(sk); 158 sock_put(sk); 159 } 160 } 161 162 static struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) 163 { 164 if (!local) 165 return NULL; 166 167 if (kref_get_unless_zero(&local->ref)) 168 return local; 169 170 return NULL; 171 } 172 173 static void local_cleanup(struct nfc_llcp_local *local) 174 { 175 nfc_llcp_socket_release(local, false, ENXIO); 176 timer_delete_sync(&local->link_timer); 177 skb_queue_purge(&local->tx_queue); 178 cancel_work_sync(&local->tx_work); 179 cancel_work_sync(&local->rx_work); 180 cancel_work_sync(&local->timeout_work); 181 kfree_skb(local->rx_pending); 182 local->rx_pending = NULL; 183 timer_delete_sync(&local->sdreq_timer); 184 cancel_work_sync(&local->sdreq_timeout_work); 185 nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs); 186 } 187 188 static void local_release_work(struct work_struct *work) 189 { 190 struct nfc_llcp_local *local; 191 struct nfc_dev *dev; 192 193 local = container_of(work, struct nfc_llcp_local, release_work); 194 dev = local->dev; 195 196 local_cleanup(local); 197 kfree(local); 198 nfc_put_device(dev); 199 } 200 201 static void local_release(struct kref *ref) 202 { 203 struct nfc_llcp_local *local; 204 205 local = container_of(ref, struct nfc_llcp_local, ref); 206 207 queue_work(llcp_wq, &local->release_work); 208 } 209 210 int nfc_llcp_local_put(struct nfc_llcp_local *local) 211 { 212 if (!local) 213 return 0; 214 215 return kref_put(&local->ref, local_release); 216 } 217 218 static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, 219 u8 ssap, u8 dsap) 220 { 221 struct sock *sk; 222 struct nfc_llcp_sock *llcp_sock, *tmp_sock; 223 224 pr_debug("ssap dsap %d %d\n", ssap, dsap); 225 226 if (ssap == 0 && dsap == 0) 227 return NULL; 228 229 read_lock(&local->sockets.lock); 230 231 llcp_sock = NULL; 232 233 sk_for_each(sk, &local->sockets.head) { 234 tmp_sock = nfc_llcp_sock(sk); 235 236 if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) { 237 llcp_sock = tmp_sock; 238 sock_hold(&llcp_sock->sk); 239 break; 240 } 241 } 242 243 read_unlock(&local->sockets.lock); 244 245 return llcp_sock; 246 } 247 248 static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) 249 { 250 sock_put(&sock->sk); 251 } 252 253 static void nfc_llcp_timeout_work(struct work_struct *work) 254 { 255 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 256 timeout_work); 257 258 nfc_dep_link_down(local->dev); 259 } 260 261 static void nfc_llcp_symm_timer(struct timer_list *t) 262 { 263 struct nfc_llcp_local *local = timer_container_of(local, t, 264 link_timer); 265 266 pr_err("SYMM timeout\n"); 267 268 schedule_work(&local->timeout_work); 269 } 270 271 static void nfc_llcp_sdreq_timeout_work(struct work_struct *work) 272 { 273 unsigned long time; 274 HLIST_HEAD(nl_sdres_list); 275 struct hlist_node *n; 276 struct nfc_llcp_sdp_tlv *sdp; 277 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 278 sdreq_timeout_work); 279 280 mutex_lock(&local->sdreq_lock); 281 282 time = jiffies - msecs_to_jiffies(3 * local->remote_lto); 283 284 hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) { 285 if (time_after(sdp->time, time)) 286 continue; 287 288 sdp->sap = LLCP_SDP_UNBOUND; 289 290 hlist_del(&sdp->node); 291 292 hlist_add_head(&sdp->node, &nl_sdres_list); 293 } 294 295 if (!hlist_empty(&local->pending_sdreqs)) 296 mod_timer(&local->sdreq_timer, 297 jiffies + msecs_to_jiffies(3 * local->remote_lto)); 298 299 mutex_unlock(&local->sdreq_lock); 300 301 if (!hlist_empty(&nl_sdres_list)) 302 nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); 303 } 304 305 static void nfc_llcp_sdreq_timer(struct timer_list *t) 306 { 307 struct nfc_llcp_local *local = timer_container_of(local, t, 308 sdreq_timer); 309 310 schedule_work(&local->sdreq_timeout_work); 311 } 312 313 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) 314 { 315 struct nfc_llcp_local *local; 316 struct nfc_llcp_local *res = NULL; 317 318 spin_lock(&llcp_devices_lock); 319 list_for_each_entry(local, &llcp_devices, list) 320 if (local->dev == dev) { 321 res = nfc_llcp_local_get(local); 322 break; 323 } 324 spin_unlock(&llcp_devices_lock); 325 326 return res; 327 } 328 329 static struct nfc_llcp_local *nfc_llcp_remove_local(struct nfc_dev *dev) 330 { 331 struct nfc_llcp_local *local, *tmp; 332 333 spin_lock(&llcp_devices_lock); 334 list_for_each_entry_safe(local, tmp, &llcp_devices, list) 335 if (local->dev == dev) { 336 spin_lock(&local->tx_queue.lock); 337 list_del_init(&local->list); 338 spin_unlock(&local->tx_queue.lock); 339 spin_unlock(&llcp_devices_lock); 340 return local; 341 } 342 spin_unlock(&llcp_devices_lock); 343 344 pr_warn("Shutting down device not found\n"); 345 346 return NULL; 347 } 348 349 static char *wks[] = { 350 NULL, 351 NULL, /* SDP */ 352 "urn:nfc:sn:ip", 353 "urn:nfc:sn:obex", 354 "urn:nfc:sn:snep", 355 }; 356 357 static int nfc_llcp_wks_sap(const char *service_name, size_t service_name_len) 358 { 359 int sap, num_wks; 360 361 pr_debug("%s\n", service_name); 362 363 if (service_name == NULL) 364 return -EINVAL; 365 366 num_wks = ARRAY_SIZE(wks); 367 368 for (sap = 0; sap < num_wks; sap++) { 369 if (wks[sap] == NULL) 370 continue; 371 372 if (strncmp(wks[sap], service_name, service_name_len) == 0) 373 return sap; 374 } 375 376 return -EINVAL; 377 } 378 379 static 380 struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local, 381 const u8 *sn, size_t sn_len, 382 bool needref) 383 { 384 struct sock *sk; 385 struct nfc_llcp_sock *llcp_sock, *tmp_sock; 386 387 pr_debug("sn %zd %p\n", sn_len, sn); 388 389 if (sn == NULL || sn_len == 0) 390 return NULL; 391 392 read_lock(&local->sockets.lock); 393 394 llcp_sock = NULL; 395 396 sk_for_each(sk, &local->sockets.head) { 397 tmp_sock = nfc_llcp_sock(sk); 398 399 pr_debug("llcp sock %p\n", tmp_sock); 400 401 if (tmp_sock->sk.sk_type == SOCK_STREAM && 402 tmp_sock->sk.sk_state != LLCP_LISTEN) 403 continue; 404 405 if (tmp_sock->sk.sk_type == SOCK_DGRAM && 406 tmp_sock->sk.sk_state != LLCP_BOUND) 407 continue; 408 409 if (tmp_sock->service_name == NULL || 410 tmp_sock->service_name_len == 0) 411 continue; 412 413 if (tmp_sock->service_name_len != sn_len) 414 continue; 415 416 if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) { 417 llcp_sock = tmp_sock; 418 if (needref) 419 sock_hold(&llcp_sock->sk); 420 break; 421 } 422 } 423 424 read_unlock(&local->sockets.lock); 425 426 pr_debug("Found llcp sock %p\n", llcp_sock); 427 428 return llcp_sock; 429 } 430 431 u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, 432 struct nfc_llcp_sock *sock) 433 { 434 mutex_lock(&local->sdp_lock); 435 436 if (sock->service_name != NULL && sock->service_name_len > 0) { 437 int ssap = nfc_llcp_wks_sap(sock->service_name, 438 sock->service_name_len); 439 440 if (ssap > 0) { 441 pr_debug("WKS %d\n", ssap); 442 443 /* This is a WKS, let's check if it's free */ 444 if (test_bit(ssap, &local->local_wks)) { 445 mutex_unlock(&local->sdp_lock); 446 447 return LLCP_SAP_MAX; 448 } 449 450 set_bit(ssap, &local->local_wks); 451 mutex_unlock(&local->sdp_lock); 452 453 return ssap; 454 } 455 456 /* 457 * Check if there already is a non WKS socket bound 458 * to this service name. 459 */ 460 if (nfc_llcp_sock_from_sn(local, sock->service_name, 461 sock->service_name_len, 462 false) != NULL) { 463 mutex_unlock(&local->sdp_lock); 464 465 return LLCP_SAP_MAX; 466 } 467 468 mutex_unlock(&local->sdp_lock); 469 470 return LLCP_SDP_UNBOUND; 471 472 } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) { 473 if (!test_bit(sock->ssap, &local->local_wks)) { 474 set_bit(sock->ssap, &local->local_wks); 475 mutex_unlock(&local->sdp_lock); 476 477 return sock->ssap; 478 } 479 } 480 481 mutex_unlock(&local->sdp_lock); 482 483 return LLCP_SAP_MAX; 484 } 485 486 u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) 487 { 488 u8 local_ssap; 489 490 mutex_lock(&local->sdp_lock); 491 492 local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); 493 if (local_ssap == LLCP_LOCAL_NUM_SAP) { 494 mutex_unlock(&local->sdp_lock); 495 return LLCP_SAP_MAX; 496 } 497 498 set_bit(local_ssap, &local->local_sap); 499 500 mutex_unlock(&local->sdp_lock); 501 502 return local_ssap + LLCP_LOCAL_SAP_OFFSET; 503 } 504 505 void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) 506 { 507 u8 local_ssap; 508 unsigned long *sdp; 509 510 if (ssap < LLCP_WKS_NUM_SAP) { 511 local_ssap = ssap; 512 sdp = &local->local_wks; 513 } else if (ssap < LLCP_LOCAL_NUM_SAP) { 514 atomic_t *client_cnt; 515 516 local_ssap = ssap - LLCP_WKS_NUM_SAP; 517 sdp = &local->local_sdp; 518 client_cnt = &local->local_sdp_cnt[local_ssap]; 519 520 pr_debug("%d clients\n", atomic_read(client_cnt)); 521 522 mutex_lock(&local->sdp_lock); 523 524 if (atomic_dec_and_test(client_cnt)) { 525 struct nfc_llcp_sock *l_sock; 526 527 pr_debug("No more clients for SAP %d\n", ssap); 528 529 clear_bit(local_ssap, sdp); 530 531 /* Find the listening sock and set it back to UNBOUND */ 532 l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP); 533 if (l_sock) { 534 l_sock->ssap = LLCP_SDP_UNBOUND; 535 nfc_llcp_sock_put(l_sock); 536 } 537 } 538 539 mutex_unlock(&local->sdp_lock); 540 541 return; 542 } else if (ssap < LLCP_MAX_SAP) { 543 local_ssap = ssap - LLCP_LOCAL_NUM_SAP; 544 sdp = &local->local_sap; 545 } else { 546 return; 547 } 548 549 mutex_lock(&local->sdp_lock); 550 551 clear_bit(local_ssap, sdp); 552 553 mutex_unlock(&local->sdp_lock); 554 } 555 556 static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local) 557 { 558 u8 ssap; 559 560 mutex_lock(&local->sdp_lock); 561 562 ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); 563 if (ssap == LLCP_SDP_NUM_SAP) { 564 mutex_unlock(&local->sdp_lock); 565 566 return LLCP_SAP_MAX; 567 } 568 569 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); 570 571 set_bit(ssap, &local->local_sdp); 572 573 mutex_unlock(&local->sdp_lock); 574 575 return LLCP_WKS_NUM_SAP + ssap; 576 } 577 578 static int nfc_llcp_build_gb(struct nfc_llcp_local *local) 579 { 580 u8 *gb_cur, version, version_length; 581 u8 lto_length, wks_length, miux_length; 582 const u8 *version_tlv = NULL, *lto_tlv = NULL, 583 *wks_tlv = NULL, *miux_tlv = NULL; 584 __be16 wks = cpu_to_be16(local->local_wks); 585 u8 gb_len = 0; 586 int ret = 0; 587 588 version = LLCP_VERSION_11; 589 version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, 590 1, &version_length); 591 if (!version_tlv) { 592 ret = -ENOMEM; 593 goto out; 594 } 595 gb_len += version_length; 596 597 lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, <o_length); 598 if (!lto_tlv) { 599 ret = -ENOMEM; 600 goto out; 601 } 602 gb_len += lto_length; 603 604 pr_debug("Local wks 0x%lx\n", local->local_wks); 605 wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&wks, 2, &wks_length); 606 if (!wks_tlv) { 607 ret = -ENOMEM; 608 goto out; 609 } 610 gb_len += wks_length; 611 612 miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0, 613 &miux_length); 614 if (!miux_tlv) { 615 ret = -ENOMEM; 616 goto out; 617 } 618 gb_len += miux_length; 619 620 gb_len += ARRAY_SIZE(llcp_magic); 621 622 if (gb_len > NFC_MAX_GT_LEN) { 623 ret = -EINVAL; 624 goto out; 625 } 626 627 gb_cur = local->gb; 628 629 memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); 630 gb_cur += ARRAY_SIZE(llcp_magic); 631 632 memcpy(gb_cur, version_tlv, version_length); 633 gb_cur += version_length; 634 635 memcpy(gb_cur, lto_tlv, lto_length); 636 gb_cur += lto_length; 637 638 memcpy(gb_cur, wks_tlv, wks_length); 639 gb_cur += wks_length; 640 641 memcpy(gb_cur, miux_tlv, miux_length); 642 gb_cur += miux_length; 643 644 local->gb_len = gb_len; 645 646 out: 647 kfree(version_tlv); 648 kfree(lto_tlv); 649 kfree(wks_tlv); 650 kfree(miux_tlv); 651 652 return ret; 653 } 654 655 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) 656 { 657 struct nfc_llcp_local *local; 658 659 local = nfc_llcp_find_local(dev); 660 if (local == NULL) { 661 *general_bytes_len = 0; 662 return NULL; 663 } 664 665 nfc_llcp_build_gb(local); 666 667 *general_bytes_len = local->gb_len; 668 669 nfc_llcp_local_put(local); 670 671 return local->gb; 672 } 673 674 int nfc_llcp_set_remote_gb(struct nfc_dev *dev, const u8 *gb, u8 gb_len) 675 { 676 struct nfc_llcp_local *local; 677 int err; 678 679 if (gb_len < 3 || gb_len > NFC_MAX_GT_LEN) 680 return -EINVAL; 681 682 local = nfc_llcp_find_local(dev); 683 if (local == NULL) { 684 pr_err("No LLCP device\n"); 685 return -ENODEV; 686 } 687 688 memset(local->remote_gb, 0, NFC_MAX_GT_LEN); 689 memcpy(local->remote_gb, gb, gb_len); 690 local->remote_gb_len = gb_len; 691 692 if (memcmp(local->remote_gb, llcp_magic, 3)) { 693 pr_err("MAC does not support LLCP\n"); 694 err = -EINVAL; 695 goto out; 696 } 697 698 err = nfc_llcp_parse_gb_tlv(local, 699 &local->remote_gb[3], 700 local->remote_gb_len - 3); 701 out: 702 nfc_llcp_local_put(local); 703 return err; 704 } 705 706 static u8 nfc_llcp_dsap(const struct sk_buff *pdu) 707 { 708 return (pdu->data[0] & 0xfc) >> 2; 709 } 710 711 static u8 nfc_llcp_ptype(const struct sk_buff *pdu) 712 { 713 return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); 714 } 715 716 static u8 nfc_llcp_ssap(const struct sk_buff *pdu) 717 { 718 return pdu->data[1] & 0x3f; 719 } 720 721 static u8 nfc_llcp_ns(const struct sk_buff *pdu) 722 { 723 return pdu->data[2] >> 4; 724 } 725 726 static u8 nfc_llcp_nr(const struct sk_buff *pdu) 727 { 728 return pdu->data[2] & 0xf; 729 } 730 731 static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) 732 { 733 pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); 734 sock->send_n = (sock->send_n + 1) % 16; 735 sock->recv_ack_n = (sock->recv_n - 1) % 16; 736 } 737 738 void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local, 739 struct sk_buff *skb, u8 direction) 740 { 741 struct sk_buff *skb_copy = NULL, *nskb; 742 struct sock *sk; 743 u8 *data; 744 745 read_lock(&local->raw_sockets.lock); 746 747 sk_for_each(sk, &local->raw_sockets.head) { 748 if (sk->sk_state != LLCP_BOUND) 749 continue; 750 751 if (skb_copy == NULL) { 752 skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE, 753 GFP_ATOMIC, true); 754 755 if (skb_copy == NULL) 756 continue; 757 758 data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE); 759 760 data[0] = local->dev ? local->dev->idx : 0xFF; 761 data[1] = direction & 0x01; 762 data[1] |= (RAW_PAYLOAD_LLCP << 1); 763 } 764 765 nskb = skb_clone(skb_copy, GFP_ATOMIC); 766 if (!nskb) 767 continue; 768 769 if (sock_queue_rcv_skb(sk, nskb)) 770 kfree_skb(nskb); 771 } 772 773 read_unlock(&local->raw_sockets.lock); 774 775 kfree_skb(skb_copy); 776 } 777 778 static void nfc_llcp_tx_work(struct work_struct *work) 779 { 780 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 781 tx_work); 782 struct sk_buff *skb; 783 struct sock *sk; 784 struct nfc_llcp_sock *llcp_sock; 785 786 skb = skb_dequeue(&local->tx_queue); 787 if (skb != NULL) { 788 sk = skb->sk; 789 llcp_sock = nfc_llcp_sock(sk); 790 791 if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) { 792 kfree_skb(skb); 793 nfc_llcp_send_symm(local->dev); 794 } else if (llcp_sock && !llcp_sock->remote_ready) { 795 skb_queue_head(&local->tx_queue, skb); 796 nfc_llcp_send_symm(local->dev); 797 } else { 798 struct sk_buff *copy_skb = NULL; 799 u8 ptype = nfc_llcp_ptype(skb); 800 int ret; 801 802 pr_debug("Sending pending skb\n"); 803 print_hex_dump_debug("LLCP Tx: ", DUMP_PREFIX_OFFSET, 804 16, 1, skb->data, skb->len, true); 805 806 if (ptype == LLCP_PDU_I) 807 copy_skb = skb_copy(skb, GFP_ATOMIC); 808 809 __net_timestamp(skb); 810 811 nfc_llcp_send_to_raw_sock(local, skb, 812 NFC_DIRECTION_TX); 813 814 ret = nfc_data_exchange(local->dev, local->target_idx, 815 skb, nfc_llcp_recv, local); 816 817 if (ret) { 818 kfree_skb(copy_skb); 819 goto out; 820 } 821 822 if (ptype == LLCP_PDU_I && copy_skb) 823 skb_queue_tail(&llcp_sock->tx_pending_queue, 824 copy_skb); 825 } 826 } else { 827 nfc_llcp_send_symm(local->dev); 828 } 829 830 out: 831 mod_timer(&local->link_timer, 832 jiffies + msecs_to_jiffies(2 * local->remote_lto)); 833 } 834 835 static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local, 836 u8 ssap) 837 { 838 struct sock *sk; 839 struct nfc_llcp_sock *llcp_sock; 840 841 read_lock(&local->connecting_sockets.lock); 842 843 sk_for_each(sk, &local->connecting_sockets.head) { 844 llcp_sock = nfc_llcp_sock(sk); 845 846 if (llcp_sock->ssap == ssap) { 847 sock_hold(&llcp_sock->sk); 848 goto out; 849 } 850 } 851 852 llcp_sock = NULL; 853 854 out: 855 read_unlock(&local->connecting_sockets.lock); 856 857 return llcp_sock; 858 } 859 860 static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local, 861 const u8 *sn, size_t sn_len) 862 { 863 return nfc_llcp_sock_from_sn(local, sn, sn_len, true); 864 } 865 866 static const u8 *nfc_llcp_connect_sn(const struct sk_buff *skb, size_t *sn_len) 867 { 868 u8 type, length; 869 const u8 *tlv = &skb->data[LLCP_HEADER_SIZE]; 870 const u8 *tlv_end = skb_tail_pointer(skb); 871 872 while (tlv + 2 < tlv_end) { 873 type = tlv[0]; 874 length = tlv[1]; 875 876 if (tlv + 2 + length > tlv_end) 877 break; 878 879 pr_debug("type 0x%x length %d\n", type, length); 880 881 if (type == LLCP_TLV_SN) { 882 *sn_len = length; 883 return &tlv[2]; 884 } 885 886 tlv += length + 2; 887 } 888 889 return NULL; 890 } 891 892 static void nfc_llcp_recv_ui(struct nfc_llcp_local *local, 893 struct sk_buff *skb) 894 { 895 struct nfc_llcp_sock *llcp_sock; 896 struct nfc_llcp_ui_cb *ui_cb; 897 u8 dsap, ssap; 898 899 dsap = nfc_llcp_dsap(skb); 900 ssap = nfc_llcp_ssap(skb); 901 902 ui_cb = nfc_llcp_ui_skb_cb(skb); 903 ui_cb->dsap = dsap; 904 ui_cb->ssap = ssap; 905 906 pr_debug("%d %d\n", dsap, ssap); 907 908 /* We're looking for a bound socket, not a client one */ 909 llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); 910 if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM) 911 return; 912 913 /* There is no sequence with UI frames */ 914 skb_pull(skb, LLCP_HEADER_SIZE); 915 if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { 916 /* 917 * UI frames will be freed from the socket layer, so we 918 * need to keep them alive until someone receives them. 919 */ 920 skb_get(skb); 921 } else { 922 pr_err("Receive queue is full\n"); 923 } 924 925 nfc_llcp_sock_put(llcp_sock); 926 } 927 928 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, 929 const struct sk_buff *skb) 930 { 931 struct sock *new_sk, *parent; 932 struct nfc_llcp_sock *sock, *new_sock; 933 u8 dsap, ssap, reason; 934 935 dsap = nfc_llcp_dsap(skb); 936 ssap = nfc_llcp_ssap(skb); 937 938 pr_debug("%d %d\n", dsap, ssap); 939 940 if (dsap != LLCP_SAP_SDP) { 941 sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); 942 if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) { 943 reason = LLCP_DM_NOBOUND; 944 goto fail; 945 } 946 } else { 947 const u8 *sn; 948 size_t sn_len; 949 950 sn = nfc_llcp_connect_sn(skb, &sn_len); 951 if (sn == NULL) { 952 reason = LLCP_DM_NOBOUND; 953 goto fail; 954 } 955 956 pr_debug("Service name length %zu\n", sn_len); 957 958 sock = nfc_llcp_sock_get_sn(local, sn, sn_len); 959 if (sock == NULL) { 960 reason = LLCP_DM_NOBOUND; 961 goto fail; 962 } 963 } 964 965 lock_sock(&sock->sk); 966 967 parent = &sock->sk; 968 969 if (sk_acceptq_is_full(parent)) { 970 reason = LLCP_DM_REJ; 971 release_sock(&sock->sk); 972 sock_put(&sock->sk); 973 goto fail; 974 } 975 976 if (sock->ssap == LLCP_SDP_UNBOUND) { 977 u8 ssap = nfc_llcp_reserve_sdp_ssap(local); 978 979 pr_debug("First client, reserving %d\n", ssap); 980 981 if (ssap == LLCP_SAP_MAX) { 982 reason = LLCP_DM_REJ; 983 release_sock(&sock->sk); 984 sock_put(&sock->sk); 985 goto fail; 986 } 987 988 sock->ssap = ssap; 989 } 990 991 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC, 0); 992 if (new_sk == NULL) { 993 reason = LLCP_DM_REJ; 994 release_sock(&sock->sk); 995 sock_put(&sock->sk); 996 goto fail; 997 } 998 999 new_sock = nfc_llcp_sock(new_sk); 1000 1001 new_sock->local = nfc_llcp_local_get(local); 1002 if (!new_sock->local) { 1003 reason = LLCP_DM_REJ; 1004 sock_put(&new_sock->sk); 1005 release_sock(&sock->sk); 1006 sock_put(&sock->sk); 1007 goto fail; 1008 } 1009 1010 new_sock->dev = local->dev; 1011 new_sock->rw = sock->rw; 1012 new_sock->miux = sock->miux; 1013 new_sock->nfc_protocol = sock->nfc_protocol; 1014 new_sock->dsap = ssap; 1015 new_sock->target_idx = local->target_idx; 1016 new_sock->parent = parent; 1017 new_sock->ssap = sock->ssap; 1018 if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) { 1019 atomic_t *client_count; 1020 1021 pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock); 1022 1023 client_count = 1024 &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP]; 1025 1026 atomic_inc(client_count); 1027 new_sock->reserved_ssap = sock->ssap; 1028 } 1029 1030 nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE], 1031 skb->len - LLCP_HEADER_SIZE); 1032 1033 pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); 1034 1035 nfc_llcp_sock_link(&local->sockets, new_sk); 1036 1037 nfc_llcp_accept_enqueue(&sock->sk, new_sk); 1038 1039 nfc_get_device(local->dev->idx); 1040 1041 new_sk->sk_state = LLCP_CONNECTED; 1042 1043 /* Wake the listening processes */ 1044 parent->sk_data_ready(parent); 1045 1046 /* Send CC */ 1047 nfc_llcp_send_cc(new_sock); 1048 1049 release_sock(&sock->sk); 1050 sock_put(&sock->sk); 1051 1052 return; 1053 1054 fail: 1055 /* Send DM */ 1056 nfc_llcp_send_dm(local, dsap, ssap, reason); 1057 } 1058 1059 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) 1060 { 1061 int nr_frames = 0; 1062 struct nfc_llcp_local *local = sock->local; 1063 1064 pr_debug("Remote ready %d tx queue len %d remote rw %d", 1065 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), 1066 sock->remote_rw); 1067 1068 /* Try to queue some I frames for transmission */ 1069 while (sock->remote_ready && 1070 skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) { 1071 struct sk_buff *pdu; 1072 1073 pdu = skb_dequeue(&sock->tx_queue); 1074 if (pdu == NULL) 1075 break; 1076 1077 /* Update N(S)/N(R) */ 1078 nfc_llcp_set_nrns(sock, pdu); 1079 1080 skb_queue_tail(&local->tx_queue, pdu); 1081 nr_frames++; 1082 } 1083 1084 return nr_frames; 1085 } 1086 1087 static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, 1088 struct sk_buff *skb) 1089 { 1090 struct nfc_llcp_sock *llcp_sock; 1091 struct sock *sk; 1092 u8 dsap, ssap, ptype, ns, nr; 1093 1094 ptype = nfc_llcp_ptype(skb); 1095 dsap = nfc_llcp_dsap(skb); 1096 ssap = nfc_llcp_ssap(skb); 1097 ns = nfc_llcp_ns(skb); 1098 nr = nfc_llcp_nr(skb); 1099 1100 pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); 1101 1102 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); 1103 if (llcp_sock == NULL) { 1104 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); 1105 return; 1106 } 1107 1108 sk = &llcp_sock->sk; 1109 lock_sock(sk); 1110 if (sk->sk_state == LLCP_CLOSED) { 1111 release_sock(sk); 1112 nfc_llcp_sock_put(llcp_sock); 1113 return; 1114 } 1115 1116 /* Pass the payload upstream */ 1117 if (ptype == LLCP_PDU_I) { 1118 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); 1119 1120 if (ns == llcp_sock->recv_n) 1121 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; 1122 else 1123 pr_err("Received out of sequence I PDU\n"); 1124 1125 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); 1126 if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { 1127 /* 1128 * I frames will be freed from the socket layer, so we 1129 * need to keep them alive until someone receives them. 1130 */ 1131 skb_get(skb); 1132 } else { 1133 pr_err("Receive queue is full\n"); 1134 } 1135 } 1136 1137 /* Remove skbs from the pending queue */ 1138 if (llcp_sock->send_ack_n != nr) { 1139 struct sk_buff *s, *tmp; 1140 u8 n; 1141 1142 llcp_sock->send_ack_n = nr; 1143 1144 /* Remove and free all skbs until ns == nr */ 1145 skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) { 1146 n = nfc_llcp_ns(s); 1147 1148 skb_unlink(s, &llcp_sock->tx_pending_queue); 1149 kfree_skb(s); 1150 1151 if (n == nr) 1152 break; 1153 } 1154 1155 /* Re-queue the remaining skbs for transmission */ 1156 skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue, 1157 s, tmp) { 1158 skb_unlink(s, &llcp_sock->tx_pending_queue); 1159 skb_queue_head(&local->tx_queue, s); 1160 } 1161 } 1162 1163 if (ptype == LLCP_PDU_RR) 1164 llcp_sock->remote_ready = true; 1165 else if (ptype == LLCP_PDU_RNR) 1166 llcp_sock->remote_ready = false; 1167 1168 if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I) 1169 nfc_llcp_send_rr(llcp_sock); 1170 1171 release_sock(sk); 1172 nfc_llcp_sock_put(llcp_sock); 1173 } 1174 1175 static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, 1176 const struct sk_buff *skb) 1177 { 1178 struct nfc_llcp_sock *llcp_sock; 1179 struct sock *sk; 1180 u8 dsap, ssap; 1181 1182 dsap = nfc_llcp_dsap(skb); 1183 ssap = nfc_llcp_ssap(skb); 1184 1185 if ((dsap == 0) && (ssap == 0)) { 1186 pr_debug("Connection termination"); 1187 nfc_dep_link_down(local->dev); 1188 return; 1189 } 1190 1191 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); 1192 if (llcp_sock == NULL) { 1193 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); 1194 return; 1195 } 1196 1197 sk = &llcp_sock->sk; 1198 lock_sock(sk); 1199 1200 nfc_llcp_socket_purge(llcp_sock); 1201 1202 if (sk->sk_state == LLCP_CLOSED) { 1203 release_sock(sk); 1204 nfc_llcp_sock_put(llcp_sock); 1205 return; 1206 } 1207 1208 if (sk->sk_state == LLCP_CONNECTED) { 1209 nfc_put_device(local->dev); 1210 sk->sk_state = LLCP_CLOSED; 1211 sk->sk_state_change(sk); 1212 } 1213 1214 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); 1215 1216 release_sock(sk); 1217 nfc_llcp_sock_put(llcp_sock); 1218 } 1219 1220 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, 1221 const struct sk_buff *skb) 1222 { 1223 struct nfc_llcp_sock *llcp_sock; 1224 struct sock *sk; 1225 u8 dsap, ssap; 1226 1227 dsap = nfc_llcp_dsap(skb); 1228 ssap = nfc_llcp_ssap(skb); 1229 1230 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); 1231 if (llcp_sock == NULL) { 1232 pr_err("Invalid CC\n"); 1233 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); 1234 1235 return; 1236 } 1237 1238 sk = &llcp_sock->sk; 1239 1240 lock_sock(sk); 1241 1242 /* Check if socket was destroyed whilst waiting for the lock */ 1243 if (!sk_hashed(sk)) { 1244 release_sock(sk); 1245 nfc_llcp_sock_put(llcp_sock); 1246 return; 1247 } 1248 1249 /* Unlink from connecting and link to the client array */ 1250 nfc_llcp_sock_unlink(&local->connecting_sockets, sk); 1251 nfc_llcp_sock_link(&local->sockets, sk); 1252 llcp_sock->dsap = ssap; 1253 1254 nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], 1255 skb->len - LLCP_HEADER_SIZE); 1256 1257 sk->sk_state = LLCP_CONNECTED; 1258 sk->sk_state_change(sk); 1259 1260 release_sock(sk); 1261 1262 nfc_llcp_sock_put(llcp_sock); 1263 } 1264 1265 static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, 1266 const struct sk_buff *skb) 1267 { 1268 struct nfc_llcp_sock *llcp_sock; 1269 struct sock *sk; 1270 u8 dsap, ssap, reason; 1271 bool connecting = false; 1272 1273 dsap = nfc_llcp_dsap(skb); 1274 ssap = nfc_llcp_ssap(skb); 1275 reason = skb->data[2]; 1276 1277 pr_debug("%d %d reason %d\n", ssap, dsap, reason); 1278 1279 switch (reason) { 1280 case LLCP_DM_NOBOUND: 1281 case LLCP_DM_REJ: 1282 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); 1283 connecting = true; 1284 break; 1285 1286 default: 1287 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); 1288 break; 1289 } 1290 1291 if (llcp_sock == NULL) { 1292 pr_debug("Already closed\n"); 1293 return; 1294 } 1295 1296 sk = &llcp_sock->sk; 1297 1298 lock_sock(sk); 1299 1300 /* Check if socket was destroyed whilst waiting for the lock */ 1301 if (!sk_hashed(sk)) { 1302 release_sock(sk); 1303 nfc_llcp_sock_put(llcp_sock); 1304 return; 1305 } 1306 1307 /* 1308 * For DM(NOBOUND)/DM(REJ) the socket is still linked on the 1309 * connecting_sockets list. Unlink it here, under the socket lock, 1310 * before moving it to LLCP_CLOSED: llcp_sock_release() selects the 1311 * list to unlink from by sk_state, so leaving a connecting socket 1312 * in the CLOSED state would make it unlink from the wrong list and 1313 * corrupt the connecting_sockets list / desync the socket refcount. 1314 * This mirrors nfc_llcp_recv_cc(). 1315 */ 1316 if (connecting) 1317 nfc_llcp_sock_unlink(&local->connecting_sockets, sk); 1318 1319 sk->sk_err = ENXIO; 1320 sk->sk_state = LLCP_CLOSED; 1321 sk->sk_state_change(sk); 1322 1323 release_sock(sk); 1324 1325 nfc_llcp_sock_put(llcp_sock); 1326 } 1327 1328 static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 1329 const struct sk_buff *skb) 1330 { 1331 struct nfc_llcp_sock *llcp_sock; 1332 u8 dsap, ssap, type, length, tid, sap; 1333 const u8 *tlv, *tlv_end; 1334 const char *service_name; 1335 int service_name_len; 1336 struct nfc_llcp_sdp_tlv *sdp; 1337 HLIST_HEAD(llc_sdres_list); 1338 size_t sdres_tlvs_len; 1339 HLIST_HEAD(nl_sdres_list); 1340 1341 dsap = nfc_llcp_dsap(skb); 1342 ssap = nfc_llcp_ssap(skb); 1343 1344 pr_debug("%d %d\n", dsap, ssap); 1345 1346 if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) { 1347 pr_err("Wrong SNL SAP\n"); 1348 return; 1349 } 1350 1351 /* 1352 * Walk the SNL TLV list in the linear part of the skb only, 1353 * bounded by skb_tail_pointer(). Each TLV needs a two-byte 1354 * header (type, length) and its declared length must fit before 1355 * the end; this also keeps the walk safe for very short frames. 1356 */ 1357 tlv = &skb->data[LLCP_HEADER_SIZE]; 1358 tlv_end = skb_tail_pointer(skb); 1359 sdres_tlvs_len = 0; 1360 1361 while (tlv + 2 < tlv_end) { 1362 type = tlv[0]; 1363 length = tlv[1]; 1364 1365 if (tlv + 2 + length > tlv_end) 1366 break; 1367 1368 switch (type) { 1369 case LLCP_TLV_SDREQ: 1370 if (length < 1) 1371 break; 1372 1373 tid = tlv[2]; 1374 service_name = (char *) &tlv[3]; 1375 service_name_len = length - 1; 1376 1377 pr_debug("Looking for %.*s\n", service_name_len, 1378 service_name); 1379 1380 if (service_name_len == strlen("urn:nfc:sn:sdp") && 1381 !strncmp(service_name, "urn:nfc:sn:sdp", 1382 service_name_len)) { 1383 sap = 1; 1384 goto add_snl; 1385 } 1386 1387 llcp_sock = nfc_llcp_sock_from_sn(local, service_name, 1388 service_name_len, 1389 true); 1390 if (!llcp_sock) { 1391 sap = 0; 1392 goto add_snl; 1393 } 1394 1395 /* 1396 * We found a socket but its ssap has not been reserved 1397 * yet. We need to assign it for good and send a reply. 1398 * The ssap will be freed when the socket is closed. 1399 */ 1400 if (llcp_sock->ssap == LLCP_SDP_UNBOUND) { 1401 atomic_t *client_count; 1402 1403 sap = nfc_llcp_reserve_sdp_ssap(local); 1404 1405 pr_debug("Reserving %d\n", sap); 1406 1407 if (sap == LLCP_SAP_MAX) { 1408 sap = 0; 1409 nfc_llcp_sock_put(llcp_sock); 1410 goto add_snl; 1411 } 1412 1413 client_count = 1414 &local->local_sdp_cnt[sap - 1415 LLCP_WKS_NUM_SAP]; 1416 1417 atomic_inc(client_count); 1418 1419 llcp_sock->ssap = sap; 1420 llcp_sock->reserved_ssap = sap; 1421 } else { 1422 sap = llcp_sock->ssap; 1423 } 1424 1425 pr_debug("%p %d\n", llcp_sock, sap); 1426 1427 nfc_llcp_sock_put(llcp_sock); 1428 add_snl: 1429 sdp = nfc_llcp_build_sdres_tlv(tid, sap); 1430 if (sdp == NULL) 1431 goto exit; 1432 1433 sdres_tlvs_len += sdp->tlv_len; 1434 hlist_add_head(&sdp->node, &llc_sdres_list); 1435 break; 1436 1437 case LLCP_TLV_SDRES: 1438 if (length != 2) 1439 break; 1440 1441 mutex_lock(&local->sdreq_lock); 1442 1443 pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]); 1444 1445 hlist_for_each_entry(sdp, &local->pending_sdreqs, node) { 1446 if (sdp->tid != tlv[2]) 1447 continue; 1448 1449 sdp->sap = tlv[3]; 1450 1451 pr_debug("Found: uri=%s, sap=%d\n", 1452 sdp->uri, sdp->sap); 1453 1454 hlist_del(&sdp->node); 1455 1456 hlist_add_head(&sdp->node, &nl_sdres_list); 1457 1458 break; 1459 } 1460 1461 mutex_unlock(&local->sdreq_lock); 1462 break; 1463 1464 default: 1465 pr_err("Invalid SNL tlv value 0x%x\n", type); 1466 break; 1467 } 1468 1469 tlv += length + 2; 1470 } 1471 1472 exit: 1473 if (!hlist_empty(&nl_sdres_list)) 1474 nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); 1475 1476 if (!hlist_empty(&llc_sdres_list)) 1477 nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); 1478 } 1479 1480 static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb) 1481 { 1482 u8 ptype; 1483 u16 pdu_len; 1484 struct sk_buff *new_skb; 1485 1486 if (skb->len <= LLCP_HEADER_SIZE) { 1487 pr_err("Malformed AGF PDU\n"); 1488 return; 1489 } 1490 1491 skb_pull(skb, LLCP_HEADER_SIZE); 1492 1493 while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) { 1494 pdu_len = skb->data[0] << 8 | skb->data[1]; 1495 1496 skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE); 1497 1498 if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) { 1499 pr_err("Malformed AGF PDU\n"); 1500 return; 1501 } 1502 1503 ptype = nfc_llcp_ptype(skb); 1504 1505 if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) 1506 goto next; 1507 1508 new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL); 1509 if (new_skb == NULL) { 1510 pr_err("Could not allocate PDU\n"); 1511 return; 1512 } 1513 1514 skb_put_data(new_skb, skb->data, pdu_len); 1515 1516 nfc_llcp_rx_skb(local, new_skb); 1517 1518 kfree_skb(new_skb); 1519 next: 1520 skb_pull(skb, pdu_len); 1521 } 1522 } 1523 1524 static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb) 1525 { 1526 u8 dsap, ssap, ptype; 1527 1528 ptype = nfc_llcp_ptype(skb); 1529 dsap = nfc_llcp_dsap(skb); 1530 ssap = nfc_llcp_ssap(skb); 1531 1532 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); 1533 1534 if (ptype != LLCP_PDU_SYMM) 1535 print_hex_dump_debug("LLCP Rx: ", DUMP_PREFIX_OFFSET, 16, 1, 1536 skb->data, skb->len, true); 1537 1538 switch (ptype) { 1539 case LLCP_PDU_SYMM: 1540 pr_debug("SYMM\n"); 1541 break; 1542 1543 case LLCP_PDU_UI: 1544 pr_debug("UI\n"); 1545 nfc_llcp_recv_ui(local, skb); 1546 break; 1547 1548 case LLCP_PDU_CONNECT: 1549 pr_debug("CONNECT\n"); 1550 nfc_llcp_recv_connect(local, skb); 1551 break; 1552 1553 case LLCP_PDU_DISC: 1554 pr_debug("DISC\n"); 1555 nfc_llcp_recv_disc(local, skb); 1556 break; 1557 1558 case LLCP_PDU_CC: 1559 pr_debug("CC\n"); 1560 nfc_llcp_recv_cc(local, skb); 1561 break; 1562 1563 case LLCP_PDU_DM: 1564 pr_debug("DM\n"); 1565 nfc_llcp_recv_dm(local, skb); 1566 break; 1567 1568 case LLCP_PDU_SNL: 1569 pr_debug("SNL\n"); 1570 nfc_llcp_recv_snl(local, skb); 1571 break; 1572 1573 case LLCP_PDU_I: 1574 case LLCP_PDU_RR: 1575 case LLCP_PDU_RNR: 1576 pr_debug("I frame\n"); 1577 nfc_llcp_recv_hdlc(local, skb); 1578 break; 1579 1580 case LLCP_PDU_AGF: 1581 pr_debug("AGF frame\n"); 1582 nfc_llcp_recv_agf(local, skb); 1583 break; 1584 } 1585 } 1586 1587 static void nfc_llcp_rx_work(struct work_struct *work) 1588 { 1589 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 1590 rx_work); 1591 struct sk_buff *skb; 1592 1593 skb = local->rx_pending; 1594 if (skb == NULL) { 1595 pr_debug("No pending SKB\n"); 1596 return; 1597 } 1598 1599 __net_timestamp(skb); 1600 1601 nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_RX); 1602 1603 nfc_llcp_rx_skb(local, skb); 1604 1605 schedule_work(&local->tx_work); 1606 kfree_skb(local->rx_pending); 1607 local->rx_pending = NULL; 1608 } 1609 1610 static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb) 1611 { 1612 if (!pskb_may_pull(skb, LLCP_HEADER_SIZE)) { 1613 kfree_skb(skb); 1614 return; 1615 } 1616 1617 local->rx_pending = skb; 1618 timer_delete(&local->link_timer); 1619 schedule_work(&local->rx_work); 1620 } 1621 1622 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) 1623 { 1624 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; 1625 1626 if (err < 0) { 1627 pr_err("LLCP PDU receive err %d\n", err); 1628 return; 1629 } 1630 1631 __nfc_llcp_recv(local, skb); 1632 } 1633 1634 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb) 1635 { 1636 struct nfc_llcp_local *local; 1637 1638 local = nfc_llcp_find_local(dev); 1639 if (local == NULL) { 1640 kfree_skb(skb); 1641 return -ENODEV; 1642 } 1643 1644 __nfc_llcp_recv(local, skb); 1645 1646 nfc_llcp_local_put(local); 1647 1648 return 0; 1649 } 1650 1651 void nfc_llcp_mac_is_down(struct nfc_dev *dev) 1652 { 1653 struct nfc_llcp_local *local; 1654 1655 local = nfc_llcp_find_local(dev); 1656 if (local == NULL) 1657 return; 1658 1659 local->remote_miu = LLCP_DEFAULT_MIU; 1660 local->remote_lto = LLCP_DEFAULT_LTO; 1661 1662 /* Close and purge all existing sockets */ 1663 nfc_llcp_socket_release(local, true, 0); 1664 1665 nfc_llcp_local_put(local); 1666 } 1667 1668 void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, 1669 u8 comm_mode, u8 rf_mode) 1670 { 1671 struct nfc_llcp_local *local; 1672 1673 pr_debug("rf mode %d\n", rf_mode); 1674 1675 local = nfc_llcp_find_local(dev); 1676 if (local == NULL) 1677 return; 1678 1679 local->target_idx = target_idx; 1680 local->comm_mode = comm_mode; 1681 local->rf_mode = rf_mode; 1682 1683 if (rf_mode == NFC_RF_INITIATOR) { 1684 pr_debug("Queueing Tx work\n"); 1685 1686 schedule_work(&local->tx_work); 1687 } else { 1688 mod_timer(&local->link_timer, 1689 jiffies + msecs_to_jiffies(local->remote_lto)); 1690 } 1691 1692 nfc_llcp_local_put(local); 1693 } 1694 1695 int nfc_llcp_register_device(struct nfc_dev *ndev) 1696 { 1697 struct nfc_llcp_local *local; 1698 1699 local = kzalloc_obj(struct nfc_llcp_local); 1700 if (local == NULL) 1701 return -ENOMEM; 1702 1703 /* As we are going to initialize local's refcount, we need to get the 1704 * nfc_dev to avoid UAF, otherwise there is no point in continuing. 1705 * See nfc_llcp_local_get(). 1706 */ 1707 local->dev = nfc_get_device(ndev->idx); 1708 if (!local->dev) { 1709 kfree(local); 1710 return -ENODEV; 1711 } 1712 1713 INIT_LIST_HEAD(&local->list); 1714 kref_init(&local->ref); 1715 mutex_init(&local->sdp_lock); 1716 timer_setup(&local->link_timer, nfc_llcp_symm_timer, 0); 1717 1718 skb_queue_head_init(&local->tx_queue); 1719 INIT_WORK(&local->tx_work, nfc_llcp_tx_work); 1720 1721 local->rx_pending = NULL; 1722 INIT_WORK(&local->rx_work, nfc_llcp_rx_work); 1723 1724 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); 1725 INIT_WORK(&local->release_work, local_release_work); 1726 1727 rwlock_init(&local->sockets.lock); 1728 rwlock_init(&local->connecting_sockets.lock); 1729 rwlock_init(&local->raw_sockets.lock); 1730 1731 local->lto = 150; /* 1500 ms */ 1732 local->rw = LLCP_MAX_RW; 1733 local->miux = cpu_to_be16(LLCP_MAX_MIUX); 1734 local->local_wks = 0x1; /* LLC Link Management */ 1735 1736 nfc_llcp_build_gb(local); 1737 1738 local->remote_miu = LLCP_DEFAULT_MIU; 1739 local->remote_lto = LLCP_DEFAULT_LTO; 1740 1741 mutex_init(&local->sdreq_lock); 1742 INIT_HLIST_HEAD(&local->pending_sdreqs); 1743 timer_setup(&local->sdreq_timer, nfc_llcp_sdreq_timer, 0); 1744 INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work); 1745 1746 spin_lock(&llcp_devices_lock); 1747 list_add(&local->list, &llcp_devices); 1748 spin_unlock(&llcp_devices_lock); 1749 1750 return 0; 1751 } 1752 1753 void nfc_llcp_unregister_device(struct nfc_dev *dev) 1754 { 1755 struct nfc_llcp_local *local = nfc_llcp_remove_local(dev); 1756 1757 if (local == NULL) { 1758 pr_debug("No such device\n"); 1759 return; 1760 } 1761 1762 local_cleanup(local); 1763 1764 nfc_llcp_local_put(local); 1765 } 1766 1767 int __init nfc_llcp_init(void) 1768 { 1769 int ret; 1770 1771 llcp_wq = alloc_workqueue("nfc_llcp_wq", WQ_UNBOUND, 0); 1772 if (!llcp_wq) 1773 return -ENOMEM; 1774 1775 ret = nfc_llcp_sock_init(); 1776 if (ret) { 1777 destroy_workqueue(llcp_wq); 1778 return ret; 1779 } 1780 1781 return 0; 1782 } 1783 1784 void nfc_llcp_exit(void) 1785 { 1786 nfc_llcp_sock_exit(); 1787 destroy_workqueue(llcp_wq); 1788 } 1789