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