net/nfc/llcp_core.c

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