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