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