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(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 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 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, 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(struct sk_buff *pdu) 643 { 644 return (pdu->data[0] & 0xfc) >> 2; 645 } 646 647 static u8 nfc_llcp_ptype(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(struct sk_buff *pdu) 653 { 654 return pdu->data[1] & 0x3f; 655 } 656 657 static u8 nfc_llcp_ns(struct sk_buff *pdu) 658 { 659 return pdu->data[2] >> 4; 660 } 661 662 static u8 nfc_llcp_nr(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 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 u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) 819 { 820 u8 *tlv = &skb->data[2], type, length; 821 size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; 822 823 while (offset < tlv_array_len) { 824 type = tlv[0]; 825 length = tlv[1]; 826 827 pr_debug("type 0x%x length %d\n", type, length); 828 829 if (type == LLCP_TLV_SN) { 830 *sn_len = length; 831 return &tlv[2]; 832 } 833 834 offset += length + 2; 835 tlv += length + 2; 836 } 837 838 return NULL; 839 } 840 841 static void nfc_llcp_recv_ui(struct nfc_llcp_local *local, 842 struct sk_buff *skb) 843 { 844 struct nfc_llcp_sock *llcp_sock; 845 struct nfc_llcp_ui_cb *ui_cb; 846 u8 dsap, ssap; 847 848 dsap = nfc_llcp_dsap(skb); 849 ssap = nfc_llcp_ssap(skb); 850 851 ui_cb = nfc_llcp_ui_skb_cb(skb); 852 ui_cb->dsap = dsap; 853 ui_cb->ssap = ssap; 854 855 pr_debug("%d %d\n", dsap, ssap); 856 857 /* We're looking for a bound socket, not a client one */ 858 llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); 859 if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM) 860 return; 861 862 /* There is no sequence with UI frames */ 863 skb_pull(skb, LLCP_HEADER_SIZE); 864 if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { 865 /* 866 * UI frames will be freed from the socket layer, so we 867 * need to keep them alive until someone receives them. 868 */ 869 skb_get(skb); 870 } else { 871 pr_err("Receive queue is full\n"); 872 } 873 874 nfc_llcp_sock_put(llcp_sock); 875 } 876 877 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, 878 struct sk_buff *skb) 879 { 880 struct sock *new_sk, *parent; 881 struct nfc_llcp_sock *sock, *new_sock; 882 u8 dsap, ssap, reason; 883 884 dsap = nfc_llcp_dsap(skb); 885 ssap = nfc_llcp_ssap(skb); 886 887 pr_debug("%d %d\n", dsap, ssap); 888 889 if (dsap != LLCP_SAP_SDP) { 890 sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); 891 if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) { 892 reason = LLCP_DM_NOBOUND; 893 goto fail; 894 } 895 } else { 896 u8 *sn; 897 size_t sn_len; 898 899 sn = nfc_llcp_connect_sn(skb, &sn_len); 900 if (sn == NULL) { 901 reason = LLCP_DM_NOBOUND; 902 goto fail; 903 } 904 905 pr_debug("Service name length %zu\n", sn_len); 906 907 sock = nfc_llcp_sock_get_sn(local, sn, sn_len); 908 if (sock == NULL) { 909 reason = LLCP_DM_NOBOUND; 910 goto fail; 911 } 912 } 913 914 lock_sock(&sock->sk); 915 916 parent = &sock->sk; 917 918 if (sk_acceptq_is_full(parent)) { 919 reason = LLCP_DM_REJ; 920 release_sock(&sock->sk); 921 sock_put(&sock->sk); 922 goto fail; 923 } 924 925 if (sock->ssap == LLCP_SDP_UNBOUND) { 926 u8 ssap = nfc_llcp_reserve_sdp_ssap(local); 927 928 pr_debug("First client, reserving %d\n", ssap); 929 930 if (ssap == LLCP_SAP_MAX) { 931 reason = LLCP_DM_REJ; 932 release_sock(&sock->sk); 933 sock_put(&sock->sk); 934 goto fail; 935 } 936 937 sock->ssap = ssap; 938 } 939 940 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC, 0); 941 if (new_sk == NULL) { 942 reason = LLCP_DM_REJ; 943 release_sock(&sock->sk); 944 sock_put(&sock->sk); 945 goto fail; 946 } 947 948 new_sock = nfc_llcp_sock(new_sk); 949 new_sock->dev = local->dev; 950 new_sock->local = nfc_llcp_local_get(local); 951 new_sock->rw = sock->rw; 952 new_sock->miux = sock->miux; 953 new_sock->nfc_protocol = sock->nfc_protocol; 954 new_sock->dsap = ssap; 955 new_sock->target_idx = local->target_idx; 956 new_sock->parent = parent; 957 new_sock->ssap = sock->ssap; 958 if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) { 959 atomic_t *client_count; 960 961 pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock); 962 963 client_count = 964 &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP]; 965 966 atomic_inc(client_count); 967 new_sock->reserved_ssap = sock->ssap; 968 } 969 970 nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE], 971 skb->len - LLCP_HEADER_SIZE); 972 973 pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); 974 975 nfc_llcp_sock_link(&local->sockets, new_sk); 976 977 nfc_llcp_accept_enqueue(&sock->sk, new_sk); 978 979 nfc_get_device(local->dev->idx); 980 981 new_sk->sk_state = LLCP_CONNECTED; 982 983 /* Wake the listening processes */ 984 parent->sk_data_ready(parent); 985 986 /* Send CC */ 987 nfc_llcp_send_cc(new_sock); 988 989 release_sock(&sock->sk); 990 sock_put(&sock->sk); 991 992 return; 993 994 fail: 995 /* Send DM */ 996 nfc_llcp_send_dm(local, dsap, ssap, reason); 997 } 998 999 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) 1000 { 1001 int nr_frames = 0; 1002 struct nfc_llcp_local *local = sock->local; 1003 1004 pr_debug("Remote ready %d tx queue len %d remote rw %d", 1005 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), 1006 sock->remote_rw); 1007 1008 /* Try to queue some I frames for transmission */ 1009 while (sock->remote_ready && 1010 skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) { 1011 struct sk_buff *pdu; 1012 1013 pdu = skb_dequeue(&sock->tx_queue); 1014 if (pdu == NULL) 1015 break; 1016 1017 /* Update N(S)/N(R) */ 1018 nfc_llcp_set_nrns(sock, pdu); 1019 1020 skb_queue_tail(&local->tx_queue, pdu); 1021 nr_frames++; 1022 } 1023 1024 return nr_frames; 1025 } 1026 1027 static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, 1028 struct sk_buff *skb) 1029 { 1030 struct nfc_llcp_sock *llcp_sock; 1031 struct sock *sk; 1032 u8 dsap, ssap, ptype, ns, nr; 1033 1034 ptype = nfc_llcp_ptype(skb); 1035 dsap = nfc_llcp_dsap(skb); 1036 ssap = nfc_llcp_ssap(skb); 1037 ns = nfc_llcp_ns(skb); 1038 nr = nfc_llcp_nr(skb); 1039 1040 pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); 1041 1042 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); 1043 if (llcp_sock == NULL) { 1044 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); 1045 return; 1046 } 1047 1048 sk = &llcp_sock->sk; 1049 lock_sock(sk); 1050 if (sk->sk_state == LLCP_CLOSED) { 1051 release_sock(sk); 1052 nfc_llcp_sock_put(llcp_sock); 1053 } 1054 1055 /* Pass the payload upstream */ 1056 if (ptype == LLCP_PDU_I) { 1057 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); 1058 1059 if (ns == llcp_sock->recv_n) 1060 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; 1061 else 1062 pr_err("Received out of sequence I PDU\n"); 1063 1064 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); 1065 if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { 1066 /* 1067 * I frames will be freed from the socket layer, so we 1068 * need to keep them alive until someone receives them. 1069 */ 1070 skb_get(skb); 1071 } else { 1072 pr_err("Receive queue is full\n"); 1073 } 1074 } 1075 1076 /* Remove skbs from the pending queue */ 1077 if (llcp_sock->send_ack_n != nr) { 1078 struct sk_buff *s, *tmp; 1079 u8 n; 1080 1081 llcp_sock->send_ack_n = nr; 1082 1083 /* Remove and free all skbs until ns == nr */ 1084 skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) { 1085 n = nfc_llcp_ns(s); 1086 1087 skb_unlink(s, &llcp_sock->tx_pending_queue); 1088 kfree_skb(s); 1089 1090 if (n == nr) 1091 break; 1092 } 1093 1094 /* Re-queue the remaining skbs for transmission */ 1095 skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue, 1096 s, tmp) { 1097 skb_unlink(s, &llcp_sock->tx_pending_queue); 1098 skb_queue_head(&local->tx_queue, s); 1099 } 1100 } 1101 1102 if (ptype == LLCP_PDU_RR) 1103 llcp_sock->remote_ready = true; 1104 else if (ptype == LLCP_PDU_RNR) 1105 llcp_sock->remote_ready = false; 1106 1107 if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I) 1108 nfc_llcp_send_rr(llcp_sock); 1109 1110 release_sock(sk); 1111 nfc_llcp_sock_put(llcp_sock); 1112 } 1113 1114 static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, 1115 struct sk_buff *skb) 1116 { 1117 struct nfc_llcp_sock *llcp_sock; 1118 struct sock *sk; 1119 u8 dsap, ssap; 1120 1121 dsap = nfc_llcp_dsap(skb); 1122 ssap = nfc_llcp_ssap(skb); 1123 1124 if ((dsap == 0) && (ssap == 0)) { 1125 pr_debug("Connection termination"); 1126 nfc_dep_link_down(local->dev); 1127 return; 1128 } 1129 1130 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); 1131 if (llcp_sock == NULL) { 1132 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); 1133 return; 1134 } 1135 1136 sk = &llcp_sock->sk; 1137 lock_sock(sk); 1138 1139 nfc_llcp_socket_purge(llcp_sock); 1140 1141 if (sk->sk_state == LLCP_CLOSED) { 1142 release_sock(sk); 1143 nfc_llcp_sock_put(llcp_sock); 1144 } 1145 1146 if (sk->sk_state == LLCP_CONNECTED) { 1147 nfc_put_device(local->dev); 1148 sk->sk_state = LLCP_CLOSED; 1149 sk->sk_state_change(sk); 1150 } 1151 1152 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); 1153 1154 release_sock(sk); 1155 nfc_llcp_sock_put(llcp_sock); 1156 } 1157 1158 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) 1159 { 1160 struct nfc_llcp_sock *llcp_sock; 1161 struct sock *sk; 1162 u8 dsap, ssap; 1163 1164 dsap = nfc_llcp_dsap(skb); 1165 ssap = nfc_llcp_ssap(skb); 1166 1167 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); 1168 if (llcp_sock == NULL) { 1169 pr_err("Invalid CC\n"); 1170 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); 1171 1172 return; 1173 } 1174 1175 sk = &llcp_sock->sk; 1176 1177 /* Unlink from connecting and link to the client array */ 1178 nfc_llcp_sock_unlink(&local->connecting_sockets, sk); 1179 nfc_llcp_sock_link(&local->sockets, sk); 1180 llcp_sock->dsap = ssap; 1181 1182 nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], 1183 skb->len - LLCP_HEADER_SIZE); 1184 1185 sk->sk_state = LLCP_CONNECTED; 1186 sk->sk_state_change(sk); 1187 1188 nfc_llcp_sock_put(llcp_sock); 1189 } 1190 1191 static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb) 1192 { 1193 struct nfc_llcp_sock *llcp_sock; 1194 struct sock *sk; 1195 u8 dsap, ssap, reason; 1196 1197 dsap = nfc_llcp_dsap(skb); 1198 ssap = nfc_llcp_ssap(skb); 1199 reason = skb->data[2]; 1200 1201 pr_debug("%d %d reason %d\n", ssap, dsap, reason); 1202 1203 switch (reason) { 1204 case LLCP_DM_NOBOUND: 1205 case LLCP_DM_REJ: 1206 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); 1207 break; 1208 1209 default: 1210 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); 1211 break; 1212 } 1213 1214 if (llcp_sock == NULL) { 1215 pr_debug("Already closed\n"); 1216 return; 1217 } 1218 1219 sk = &llcp_sock->sk; 1220 1221 sk->sk_err = ENXIO; 1222 sk->sk_state = LLCP_CLOSED; 1223 sk->sk_state_change(sk); 1224 1225 nfc_llcp_sock_put(llcp_sock); 1226 } 1227 1228 static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 1229 struct sk_buff *skb) 1230 { 1231 struct nfc_llcp_sock *llcp_sock; 1232 u8 dsap, ssap, *tlv, type, length, tid, sap; 1233 u16 tlv_len, offset; 1234 char *service_name; 1235 size_t service_name_len; 1236 struct nfc_llcp_sdp_tlv *sdp; 1237 HLIST_HEAD(llc_sdres_list); 1238 size_t sdres_tlvs_len; 1239 HLIST_HEAD(nl_sdres_list); 1240 1241 dsap = nfc_llcp_dsap(skb); 1242 ssap = nfc_llcp_ssap(skb); 1243 1244 pr_debug("%d %d\n", dsap, ssap); 1245 1246 if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) { 1247 pr_err("Wrong SNL SAP\n"); 1248 return; 1249 } 1250 1251 tlv = &skb->data[LLCP_HEADER_SIZE]; 1252 tlv_len = skb->len - LLCP_HEADER_SIZE; 1253 offset = 0; 1254 sdres_tlvs_len = 0; 1255 1256 while (offset < tlv_len) { 1257 type = tlv[0]; 1258 length = tlv[1]; 1259 1260 switch (type) { 1261 case LLCP_TLV_SDREQ: 1262 tid = tlv[2]; 1263 service_name = (char *) &tlv[3]; 1264 service_name_len = length - 1; 1265 1266 pr_debug("Looking for %.16s\n", service_name); 1267 1268 if (service_name_len == strlen("urn:nfc:sn:sdp") && 1269 !strncmp(service_name, "urn:nfc:sn:sdp", 1270 service_name_len)) { 1271 sap = 1; 1272 goto add_snl; 1273 } 1274 1275 llcp_sock = nfc_llcp_sock_from_sn(local, service_name, 1276 service_name_len); 1277 if (!llcp_sock) { 1278 sap = 0; 1279 goto add_snl; 1280 } 1281 1282 /* 1283 * We found a socket but its ssap has not been reserved 1284 * yet. We need to assign it for good and send a reply. 1285 * The ssap will be freed when the socket is closed. 1286 */ 1287 if (llcp_sock->ssap == LLCP_SDP_UNBOUND) { 1288 atomic_t *client_count; 1289 1290 sap = nfc_llcp_reserve_sdp_ssap(local); 1291 1292 pr_debug("Reserving %d\n", sap); 1293 1294 if (sap == LLCP_SAP_MAX) { 1295 sap = 0; 1296 goto add_snl; 1297 } 1298 1299 client_count = 1300 &local->local_sdp_cnt[sap - 1301 LLCP_WKS_NUM_SAP]; 1302 1303 atomic_inc(client_count); 1304 1305 llcp_sock->ssap = sap; 1306 llcp_sock->reserved_ssap = sap; 1307 } else { 1308 sap = llcp_sock->ssap; 1309 } 1310 1311 pr_debug("%p %d\n", llcp_sock, sap); 1312 1313 add_snl: 1314 sdp = nfc_llcp_build_sdres_tlv(tid, sap); 1315 if (sdp == NULL) 1316 goto exit; 1317 1318 sdres_tlvs_len += sdp->tlv_len; 1319 hlist_add_head(&sdp->node, &llc_sdres_list); 1320 break; 1321 1322 case LLCP_TLV_SDRES: 1323 mutex_lock(&local->sdreq_lock); 1324 1325 pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]); 1326 1327 hlist_for_each_entry(sdp, &local->pending_sdreqs, node) { 1328 if (sdp->tid != tlv[2]) 1329 continue; 1330 1331 sdp->sap = tlv[3]; 1332 1333 pr_debug("Found: uri=%s, sap=%d\n", 1334 sdp->uri, sdp->sap); 1335 1336 hlist_del(&sdp->node); 1337 1338 hlist_add_head(&sdp->node, &nl_sdres_list); 1339 1340 break; 1341 } 1342 1343 mutex_unlock(&local->sdreq_lock); 1344 break; 1345 1346 default: 1347 pr_err("Invalid SNL tlv value 0x%x\n", type); 1348 break; 1349 } 1350 1351 offset += length + 2; 1352 tlv += length + 2; 1353 } 1354 1355 exit: 1356 if (!hlist_empty(&nl_sdres_list)) 1357 nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); 1358 1359 if (!hlist_empty(&llc_sdres_list)) 1360 nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); 1361 } 1362 1363 static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb) 1364 { 1365 u8 ptype; 1366 u16 pdu_len; 1367 struct sk_buff *new_skb; 1368 1369 if (skb->len <= LLCP_HEADER_SIZE) { 1370 pr_err("Malformed AGF PDU\n"); 1371 return; 1372 } 1373 1374 skb_pull(skb, LLCP_HEADER_SIZE); 1375 1376 while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) { 1377 pdu_len = skb->data[0] << 8 | skb->data[1]; 1378 1379 skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE); 1380 1381 if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) { 1382 pr_err("Malformed AGF PDU\n"); 1383 return; 1384 } 1385 1386 ptype = nfc_llcp_ptype(skb); 1387 1388 if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) 1389 goto next; 1390 1391 new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL); 1392 if (new_skb == NULL) { 1393 pr_err("Could not allocate PDU\n"); 1394 return; 1395 } 1396 1397 skb_put_data(new_skb, skb->data, pdu_len); 1398 1399 nfc_llcp_rx_skb(local, new_skb); 1400 1401 kfree_skb(new_skb); 1402 next: 1403 skb_pull(skb, pdu_len); 1404 } 1405 } 1406 1407 static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb) 1408 { 1409 u8 dsap, ssap, ptype; 1410 1411 ptype = nfc_llcp_ptype(skb); 1412 dsap = nfc_llcp_dsap(skb); 1413 ssap = nfc_llcp_ssap(skb); 1414 1415 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); 1416 1417 if (ptype != LLCP_PDU_SYMM) 1418 print_hex_dump_debug("LLCP Rx: ", DUMP_PREFIX_OFFSET, 16, 1, 1419 skb->data, skb->len, true); 1420 1421 switch (ptype) { 1422 case LLCP_PDU_SYMM: 1423 pr_debug("SYMM\n"); 1424 break; 1425 1426 case LLCP_PDU_UI: 1427 pr_debug("UI\n"); 1428 nfc_llcp_recv_ui(local, skb); 1429 break; 1430 1431 case LLCP_PDU_CONNECT: 1432 pr_debug("CONNECT\n"); 1433 nfc_llcp_recv_connect(local, skb); 1434 break; 1435 1436 case LLCP_PDU_DISC: 1437 pr_debug("DISC\n"); 1438 nfc_llcp_recv_disc(local, skb); 1439 break; 1440 1441 case LLCP_PDU_CC: 1442 pr_debug("CC\n"); 1443 nfc_llcp_recv_cc(local, skb); 1444 break; 1445 1446 case LLCP_PDU_DM: 1447 pr_debug("DM\n"); 1448 nfc_llcp_recv_dm(local, skb); 1449 break; 1450 1451 case LLCP_PDU_SNL: 1452 pr_debug("SNL\n"); 1453 nfc_llcp_recv_snl(local, skb); 1454 break; 1455 1456 case LLCP_PDU_I: 1457 case LLCP_PDU_RR: 1458 case LLCP_PDU_RNR: 1459 pr_debug("I frame\n"); 1460 nfc_llcp_recv_hdlc(local, skb); 1461 break; 1462 1463 case LLCP_PDU_AGF: 1464 pr_debug("AGF frame\n"); 1465 nfc_llcp_recv_agf(local, skb); 1466 break; 1467 } 1468 } 1469 1470 static void nfc_llcp_rx_work(struct work_struct *work) 1471 { 1472 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, 1473 rx_work); 1474 struct sk_buff *skb; 1475 1476 skb = local->rx_pending; 1477 if (skb == NULL) { 1478 pr_debug("No pending SKB\n"); 1479 return; 1480 } 1481 1482 __net_timestamp(skb); 1483 1484 nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_RX); 1485 1486 nfc_llcp_rx_skb(local, skb); 1487 1488 schedule_work(&local->tx_work); 1489 kfree_skb(local->rx_pending); 1490 local->rx_pending = NULL; 1491 } 1492 1493 static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb) 1494 { 1495 local->rx_pending = skb; 1496 del_timer(&local->link_timer); 1497 schedule_work(&local->rx_work); 1498 } 1499 1500 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) 1501 { 1502 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; 1503 1504 pr_debug("Received an LLCP PDU\n"); 1505 if (err < 0) { 1506 pr_err("err %d\n", err); 1507 return; 1508 } 1509 1510 __nfc_llcp_recv(local, skb); 1511 } 1512 1513 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb) 1514 { 1515 struct nfc_llcp_local *local; 1516 1517 local = nfc_llcp_find_local(dev); 1518 if (local == NULL) { 1519 kfree_skb(skb); 1520 return -ENODEV; 1521 } 1522 1523 __nfc_llcp_recv(local, skb); 1524 1525 return 0; 1526 } 1527 1528 void nfc_llcp_mac_is_down(struct nfc_dev *dev) 1529 { 1530 struct nfc_llcp_local *local; 1531 1532 local = nfc_llcp_find_local(dev); 1533 if (local == NULL) 1534 return; 1535 1536 local->remote_miu = LLCP_DEFAULT_MIU; 1537 local->remote_lto = LLCP_DEFAULT_LTO; 1538 1539 /* Close and purge all existing sockets */ 1540 nfc_llcp_socket_release(local, true, 0); 1541 } 1542 1543 void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, 1544 u8 comm_mode, u8 rf_mode) 1545 { 1546 struct nfc_llcp_local *local; 1547 1548 pr_debug("rf mode %d\n", rf_mode); 1549 1550 local = nfc_llcp_find_local(dev); 1551 if (local == NULL) 1552 return; 1553 1554 local->target_idx = target_idx; 1555 local->comm_mode = comm_mode; 1556 local->rf_mode = rf_mode; 1557 1558 if (rf_mode == NFC_RF_INITIATOR) { 1559 pr_debug("Queueing Tx work\n"); 1560 1561 schedule_work(&local->tx_work); 1562 } else { 1563 mod_timer(&local->link_timer, 1564 jiffies + msecs_to_jiffies(local->remote_lto)); 1565 } 1566 } 1567 1568 int nfc_llcp_register_device(struct nfc_dev *ndev) 1569 { 1570 struct nfc_llcp_local *local; 1571 1572 local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); 1573 if (local == NULL) 1574 return -ENOMEM; 1575 1576 local->dev = ndev; 1577 INIT_LIST_HEAD(&local->list); 1578 kref_init(&local->ref); 1579 mutex_init(&local->sdp_lock); 1580 timer_setup(&local->link_timer, nfc_llcp_symm_timer, 0); 1581 1582 skb_queue_head_init(&local->tx_queue); 1583 INIT_WORK(&local->tx_work, nfc_llcp_tx_work); 1584 1585 local->rx_pending = NULL; 1586 INIT_WORK(&local->rx_work, nfc_llcp_rx_work); 1587 1588 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); 1589 1590 rwlock_init(&local->sockets.lock); 1591 rwlock_init(&local->connecting_sockets.lock); 1592 rwlock_init(&local->raw_sockets.lock); 1593 1594 local->lto = 150; /* 1500 ms */ 1595 local->rw = LLCP_MAX_RW; 1596 local->miux = cpu_to_be16(LLCP_MAX_MIUX); 1597 local->local_wks = 0x1; /* LLC Link Management */ 1598 1599 nfc_llcp_build_gb(local); 1600 1601 local->remote_miu = LLCP_DEFAULT_MIU; 1602 local->remote_lto = LLCP_DEFAULT_LTO; 1603 1604 mutex_init(&local->sdreq_lock); 1605 INIT_HLIST_HEAD(&local->pending_sdreqs); 1606 timer_setup(&local->sdreq_timer, nfc_llcp_sdreq_timer, 0); 1607 INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work); 1608 1609 list_add(&local->list, &llcp_devices); 1610 1611 return 0; 1612 } 1613 1614 void nfc_llcp_unregister_device(struct nfc_dev *dev) 1615 { 1616 struct nfc_llcp_local *local = nfc_llcp_find_local(dev); 1617 1618 if (local == NULL) { 1619 pr_debug("No such device\n"); 1620 return; 1621 } 1622 1623 local_cleanup(local); 1624 1625 nfc_llcp_local_put(local); 1626 } 1627 1628 int __init nfc_llcp_init(void) 1629 { 1630 return nfc_llcp_sock_init(); 1631 } 1632 1633 void nfc_llcp_exit(void) 1634 { 1635 nfc_llcp_sock_exit(); 1636 } 1637