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