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