net/nfc/llcp/llcp.c

   1 /*
   2  * Copyright (C) 2011  Intel Corporation. All rights reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc.,
  17  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18  */
  19
  20 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
  21
  22 #include <linux/init.h>
  23 #include <linux/kernel.h>
  24 #include <linux/list.h>
  25 #include <linux/nfc.h>
  26
  27 #include "../nfc.h"
  28 #include "llcp.h"
  29
  30 static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
  31
  32 static struct list_head llcp_devices;
  33
  34 void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
  35 {
  36         write_lock(&l->lock);
  37         sk_add_node(sk, &l->head);
  38         write_unlock(&l->lock);
  39 }
  40
  41 void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
  42 {
  43         write_lock(&l->lock);
  44         sk_del_node_init(sk);
  45         write_unlock(&l->lock);
  46 }
  47
  48 static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
  49 {
  50         struct sock *sk;
  51         struct hlist_node *node, *tmp;
  52         struct nfc_llcp_sock *llcp_sock;
  53
  54         write_lock(&local->sockets.lock);
  55
  56         sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
  57                 llcp_sock = nfc_llcp_sock(sk);
  58
  59                 lock_sock(sk);
  60
  61                 if (sk->sk_state == LLCP_CONNECTED)
  62                         nfc_put_device(llcp_sock->dev);
  63
  64                 if (sk->sk_state == LLCP_LISTEN) {
  65                         struct nfc_llcp_sock *lsk, *n;
  66                         struct sock *accept_sk;
  67
  68                         list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
  69                                                  accept_queue) {
  70                                 accept_sk = &lsk->sk;
  71                                 lock_sock(accept_sk);
  72
  73                                 nfc_llcp_accept_unlink(accept_sk);
  74
  75                                 accept_sk->sk_state = LLCP_CLOSED;
  76
  77                                 release_sock(accept_sk);
  78
  79                                 sock_orphan(accept_sk);
  80                         }
  81                 }
  82
  83                 sk->sk_state = LLCP_CLOSED;
  84
  85                 release_sock(sk);
  86
  87                 sock_orphan(sk);
  88
  89                 sk_del_node_init(sk);
  90         }
  91
  92         write_unlock(&local->sockets.lock);
  93 }
  94
  95 struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
  96 {
  97         kref_get(&local->ref);
  98
  99         return local;
 100 }
 101
 102 static void local_release(struct kref *ref)
 103 {
 104         struct nfc_llcp_local *local;
 105
 106         local = container_of(ref, struct nfc_llcp_local, ref);
 107
 108         list_del(&local->list);
 109         nfc_llcp_socket_release(local);
 110         del_timer_sync(&local->link_timer);
 111         skb_queue_purge(&local->tx_queue);
 112         destroy_workqueue(local->tx_wq);
 113         destroy_workqueue(local->rx_wq);
 114         kfree_skb(local->rx_pending);
 115         kfree(local);
 116 }
 117
 118 int nfc_llcp_local_put(struct nfc_llcp_local *local)
 119 {
 120         WARN_ON(local == NULL);
 121
 122         if (local == NULL)
 123                 return 0;
 124
 125         return kref_put(&local->ref, local_release);
 126 }
 127
 128 static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
 129 {
 130         mutex_lock(&local->sdp_lock);
 131
 132         local->local_wks = 0;
 133         local->local_sdp = 0;
 134         local->local_sap = 0;
 135
 136         mutex_unlock(&local->sdp_lock);
 137 }
 138
 139 static void nfc_llcp_timeout_work(struct work_struct *work)
 140 {
 141         struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
 142                                                     timeout_work);
 143
 144         nfc_dep_link_down(local->dev);
 145 }
 146
 147 static void nfc_llcp_symm_timer(unsigned long data)
 148 {
 149         struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
 150
 151         pr_err("SYMM timeout\n");
 152
 153         queue_work(local->timeout_wq, &local->timeout_work);
 154 }
 155
 156 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
 157 {
 158         struct nfc_llcp_local *local, *n;
 159
 160         list_for_each_entry_safe(local, n, &llcp_devices, list)
 161                 if (local->dev == dev)
 162                         return local;
 163
 164         pr_debug("No device found\n");
 165
 166         return NULL;
 167 }
 168
 169 static char *wks[] = {
 170         NULL,
 171         NULL, /* SDP */
 172         "urn:nfc:sn:ip",
 173         "urn:nfc:sn:obex",
 174         "urn:nfc:sn:snep",
 175 };
 176
 177 static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
 178 {
 179         int sap, num_wks;
 180
 181         pr_debug("%s\n", service_name);
 182
 183         if (service_name == NULL)
 184                 return -EINVAL;
 185
 186         num_wks = ARRAY_SIZE(wks);
 187
 188         for (sap = 0; sap < num_wks; sap++) {
 189                 if (wks[sap] == NULL)
 190                         continue;
 191
 192                 if (strncmp(wks[sap], service_name, service_name_len) == 0)
 193                         return sap;
 194         }
 195
 196         return -EINVAL;
 197 }
 198
 199 u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
 200                          struct nfc_llcp_sock *sock)
 201 {
 202         mutex_lock(&local->sdp_lock);
 203
 204         if (sock->service_name != NULL && sock->service_name_len > 0) {
 205                 int ssap = nfc_llcp_wks_sap(sock->service_name,
 206                                             sock->service_name_len);
 207
 208                 if (ssap > 0) {
 209                         pr_debug("WKS %d\n", ssap);
 210
 211                         /* This is a WKS, let's check if it's free */
 212                         if (local->local_wks & BIT(ssap)) {
 213                                 mutex_unlock(&local->sdp_lock);
 214
 215                                 return LLCP_SAP_MAX;
 216                         }
 217
 218                         set_bit(ssap, &local->local_wks);
 219                         mutex_unlock(&local->sdp_lock);
 220
 221                         return ssap;
 222                 }
 223
 224                 /*
 225                  * This is not a well known service,
 226                  * we should try to find a local SDP free spot
 227                  */
 228                 ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
 229                 if (ssap == LLCP_SDP_NUM_SAP) {
 230                         mutex_unlock(&local->sdp_lock);
 231
 232                         return LLCP_SAP_MAX;
 233                 }
 234
 235                 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
 236
 237                 set_bit(ssap, &local->local_sdp);
 238                 mutex_unlock(&local->sdp_lock);
 239
 240                 return LLCP_WKS_NUM_SAP + ssap;
 241
 242         } else if (sock->ssap != 0) {
 243                 if (sock->ssap < LLCP_WKS_NUM_SAP) {
 244                         if (!test_bit(sock->ssap, &local->local_wks)) {
 245                                 set_bit(sock->ssap, &local->local_wks);
 246                                 mutex_unlock(&local->sdp_lock);
 247
 248                                 return sock->ssap;
 249                         }
 250
 251                 } else if (sock->ssap < LLCP_SDP_NUM_SAP) {
 252                         if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP,
 253                                       &local->local_sdp)) {
 254                                 set_bit(sock->ssap - LLCP_WKS_NUM_SAP,
 255                                         &local->local_sdp);
 256                                 mutex_unlock(&local->sdp_lock);
 257
 258                                 return sock->ssap;
 259                         }
 260                 }
 261         }
 262
 263         mutex_unlock(&local->sdp_lock);
 264
 265         return LLCP_SAP_MAX;
 266 }
 267
 268 u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
 269 {
 270         u8 local_ssap;
 271
 272         mutex_lock(&local->sdp_lock);
 273
 274         local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
 275         if (local_ssap == LLCP_LOCAL_NUM_SAP) {
 276                 mutex_unlock(&local->sdp_lock);
 277                 return LLCP_SAP_MAX;
 278         }
 279
 280         set_bit(local_ssap, &local->local_sap);
 281
 282         mutex_unlock(&local->sdp_lock);
 283
 284         return local_ssap + LLCP_LOCAL_SAP_OFFSET;
 285 }
 286
 287 void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
 288 {
 289         u8 local_ssap;
 290         unsigned long *sdp;
 291
 292         if (ssap < LLCP_WKS_NUM_SAP) {
 293                 local_ssap = ssap;
 294                 sdp = &local->local_wks;
 295         } else if (ssap < LLCP_LOCAL_NUM_SAP) {
 296                 local_ssap = ssap - LLCP_WKS_NUM_SAP;
 297                 sdp = &local->local_sdp;
 298         } else if (ssap < LLCP_MAX_SAP) {
 299                 local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
 300                 sdp = &local->local_sap;
 301         } else {
 302                 return;
 303         }
 304
 305         mutex_lock(&local->sdp_lock);
 306
 307         clear_bit(local_ssap, sdp);
 308
 309         mutex_unlock(&local->sdp_lock);
 310 }
 311
 312 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
 313 {
 314         struct nfc_llcp_local *local;
 315
 316         local = nfc_llcp_find_local(dev);
 317         if (local == NULL) {
 318                 *general_bytes_len = 0;
 319                 return NULL;
 320         }
 321
 322         *general_bytes_len = local->gb_len;
 323
 324         return local->gb;
 325 }
 326
 327 static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 328 {
 329         u8 *gb_cur, *version_tlv, version, version_length;
 330         u8 *lto_tlv, lto, lto_length;
 331         u8 *wks_tlv, wks_length;
 332         u8 *miux_tlv, miux_length;
 333         __be16 miux;
 334         u8 gb_len = 0;
 335
 336         version = LLCP_VERSION_11;
 337         version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
 338                                          1, &version_length);
 339         gb_len += version_length;
 340
 341         /* 1500 ms */
 342         lto = 150;
 343         lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &lto, 1, &lto_length);
 344         gb_len += lto_length;
 345
 346         pr_debug("Local wks 0x%lx\n", local->local_wks);
 347         wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
 348                                      &wks_length);
 349         gb_len += wks_length;
 350
 351         miux = cpu_to_be16(LLCP_MAX_MIUX);
 352         miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
 353                                       &miux_length);
 354         gb_len += miux_length;
 355
 356         gb_len += ARRAY_SIZE(llcp_magic);
 357
 358         if (gb_len > NFC_MAX_GT_LEN) {
 359                 kfree(version_tlv);
 360                 return -EINVAL;
 361         }
 362
 363         gb_cur = local->gb;
 364
 365         memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
 366         gb_cur += ARRAY_SIZE(llcp_magic);
 367
 368         memcpy(gb_cur, version_tlv, version_length);
 369         gb_cur += version_length;
 370
 371         memcpy(gb_cur, lto_tlv, lto_length);
 372         gb_cur += lto_length;
 373
 374         memcpy(gb_cur, wks_tlv, wks_length);
 375         gb_cur += wks_length;
 376
 377         memcpy(gb_cur, miux_tlv, miux_length);
 378         gb_cur += miux_length;
 379
 380         kfree(version_tlv);
 381         kfree(lto_tlv);
 382
 383         local->gb_len = gb_len;
 384
 385         return 0;
 386 }
 387
 388 int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
 389 {
 390         struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
 391
 392         if (local == NULL) {
 393                 pr_err("No LLCP device\n");
 394                 return -ENODEV;
 395         }
 396
 397         memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
 398         memcpy(local->remote_gb, gb, gb_len);
 399         local->remote_gb_len = gb_len;
 400
 401         if (local->remote_gb == NULL || local->remote_gb_len == 0)
 402                 return -ENODEV;
 403
 404         if (memcmp(local->remote_gb, llcp_magic, 3)) {
 405                 pr_err("MAC does not support LLCP\n");
 406                 return -EINVAL;
 407         }
 408
 409         return nfc_llcp_parse_gb_tlv(local,
 410                                      &local->remote_gb[3],
 411                                      local->remote_gb_len - 3);
 412 }
 413
 414 static void nfc_llcp_tx_work(struct work_struct *work)
 415 {
 416         struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
 417                                                     tx_work);
 418         struct sk_buff *skb;
 419
 420         skb = skb_dequeue(&local->tx_queue);
 421         if (skb != NULL) {
 422                 pr_debug("Sending pending skb\n");
 423                 print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET,
 424                                16, 1, skb->data, skb->len, true);
 425
 426                 nfc_data_exchange(local->dev, local->target_idx,
 427                                   skb, nfc_llcp_recv, local);
 428         } else {
 429                 nfc_llcp_send_symm(local->dev);
 430         }
 431
 432         mod_timer(&local->link_timer,
 433                   jiffies + msecs_to_jiffies(local->remote_lto));
 434 }
 435
 436 static u8 nfc_llcp_dsap(struct sk_buff *pdu)
 437 {
 438         return (pdu->data[0] & 0xfc) >> 2;
 439 }
 440
 441 static u8 nfc_llcp_ptype(struct sk_buff *pdu)
 442 {
 443         return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
 444 }
 445
 446 static u8 nfc_llcp_ssap(struct sk_buff *pdu)
 447 {
 448         return pdu->data[1] & 0x3f;
 449 }
 450
 451 static u8 nfc_llcp_ns(struct sk_buff *pdu)
 452 {
 453         return pdu->data[2] >> 4;
 454 }
 455
 456 static u8 nfc_llcp_nr(struct sk_buff *pdu)
 457 {
 458         return pdu->data[2] & 0xf;
 459 }
 460
 461 static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
 462 {
 463         pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
 464         sock->send_n = (sock->send_n + 1) % 16;
 465         sock->recv_ack_n = (sock->recv_n - 1) % 16;
 466 }
 467
 468 static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
 469                                                           u8 ssap)
 470 {
 471         struct sock *sk;
 472         struct nfc_llcp_sock *llcp_sock;
 473         struct hlist_node *node;
 474
 475         read_lock(&local->connecting_sockets.lock);
 476
 477         sk_for_each(sk, node, &local->connecting_sockets.head) {
 478                 llcp_sock = nfc_llcp_sock(sk);
 479
 480                 if (llcp_sock->ssap == ssap)
 481                         goto out;
 482         }
 483
 484         llcp_sock = NULL;
 485
 486 out:
 487         read_unlock(&local->connecting_sockets.lock);
 488
 489         sock_hold(&llcp_sock->sk);
 490
 491         return llcp_sock;
 492 }
 493
 494 static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 495                                                u8 ssap, u8 dsap)
 496 {
 497         struct sock *sk;
 498         struct hlist_node *node;
 499         struct nfc_llcp_sock *llcp_sock;
 500
 501         pr_debug("ssap dsap %d %d\n", ssap, dsap);
 502
 503         if (ssap == 0 && dsap == 0)
 504                 return NULL;
 505
 506         read_lock(&local->sockets.lock);
 507
 508         llcp_sock = NULL;
 509
 510         sk_for_each(sk, node, &local->sockets.head) {
 511                 llcp_sock = nfc_llcp_sock(sk);
 512
 513                 if (llcp_sock->ssap == ssap &&
 514                     llcp_sock->dsap == dsap)
 515                         break;
 516         }
 517
 518         read_unlock(&local->sockets.lock);
 519
 520         if (llcp_sock == NULL)
 521                 return NULL;
 522
 523         sock_hold(&llcp_sock->sk);
 524
 525         return llcp_sock;
 526 }
 527
 528 static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
 529                                                   u8 *sn, size_t sn_len)
 530 {
 531         struct sock *sk;
 532         struct hlist_node *node;
 533         struct nfc_llcp_sock *llcp_sock;
 534
 535         pr_debug("sn %zd\n", sn_len);
 536
 537         if (sn == NULL || sn_len == 0)
 538                 return NULL;
 539
 540         read_lock(&local->sockets.lock);
 541
 542         llcp_sock = NULL;
 543
 544         sk_for_each(sk, node, &local->sockets.head) {
 545                 llcp_sock = nfc_llcp_sock(sk);
 546
 547                 if (llcp_sock->sk.sk_state != LLCP_LISTEN)
 548                         continue;
 549
 550                 if (llcp_sock->service_name == NULL ||
 551                     llcp_sock->service_name_len == 0)
 552                         continue;
 553
 554                 if (llcp_sock->service_name_len != sn_len)
 555                         continue;
 556
 557                 if (memcmp(sn, llcp_sock->service_name, sn_len) == 0)
 558                         break;
 559         }
 560
 561         read_unlock(&local->sockets.lock);
 562
 563         if (llcp_sock == NULL)
 564                 return NULL;
 565
 566         sock_hold(&llcp_sock->sk);
 567
 568         return llcp_sock;
 569 }
 570
 571 static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
 572 {
 573         sock_put(&sock->sk);
 574 }
 575
 576 static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
 577 {
 578         u8 *tlv = &skb->data[2], type, length;
 579         size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
 580
 581         while (offset < tlv_array_len) {
 582                 type = tlv[0];
 583                 length = tlv[1];
 584
 585                 pr_debug("type 0x%x length %d\n", type, length);
 586
 587                 if (type == LLCP_TLV_SN) {
 588                         *sn_len = length;
 589                         return &tlv[2];
 590                 }
 591
 592                 offset += length + 2;
 593                 tlv += length + 2;
 594         }
 595
 596         return NULL;
 597 }
 598
 599 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 600                                   struct sk_buff *skb)
 601 {
 602         struct sock *new_sk, *parent;
 603         struct nfc_llcp_sock *sock, *new_sock;
 604         u8 dsap, ssap, reason;
 605
 606         dsap = nfc_llcp_dsap(skb);
 607         ssap = nfc_llcp_ssap(skb);
 608
 609         pr_debug("%d %d\n", dsap, ssap);
 610
 611         if (dsap != LLCP_SAP_SDP) {
 612                 sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
 613                 if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
 614                         reason = LLCP_DM_NOBOUND;
 615                         goto fail;
 616                 }
 617         } else {
 618                 u8 *sn;
 619                 size_t sn_len;
 620
 621                 sn = nfc_llcp_connect_sn(skb, &sn_len);
 622                 if (sn == NULL) {
 623                         reason = LLCP_DM_NOBOUND;
 624                         goto fail;
 625                 }
 626
 627                 pr_debug("Service name length %zu\n", sn_len);
 628
 629                 sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
 630                 if (sock == NULL) {
 631                         reason = LLCP_DM_NOBOUND;
 632                         goto fail;
 633                 }
 634         }
 635
 636         lock_sock(&sock->sk);
 637
 638         parent = &sock->sk;
 639
 640         if (sk_acceptq_is_full(parent)) {
 641                 reason = LLCP_DM_REJ;
 642                 release_sock(&sock->sk);
 643                 sock_put(&sock->sk);
 644                 goto fail;
 645         }
 646
 647         new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
 648         if (new_sk == NULL) {
 649                 reason = LLCP_DM_REJ;
 650                 release_sock(&sock->sk);
 651                 sock_put(&sock->sk);
 652                 goto fail;
 653         }
 654
 655         new_sock = nfc_llcp_sock(new_sk);
 656         new_sock->dev = local->dev;
 657         new_sock->local = nfc_llcp_local_get(local);
 658         new_sock->miu = local->remote_miu;
 659         new_sock->nfc_protocol = sock->nfc_protocol;
 660         new_sock->ssap = sock->ssap;
 661         new_sock->dsap = ssap;
 662         new_sock->parent = parent;
 663
 664         nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
 665                                       skb->len - LLCP_HEADER_SIZE);
 666
 667         pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
 668
 669         nfc_llcp_sock_link(&local->sockets, new_sk);
 670
 671         nfc_llcp_accept_enqueue(&sock->sk, new_sk);
 672
 673         nfc_get_device(local->dev->idx);
 674
 675         new_sk->sk_state = LLCP_CONNECTED;
 676
 677         /* Wake the listening processes */
 678         parent->sk_data_ready(parent, 0);
 679
 680         /* Send CC */
 681         nfc_llcp_send_cc(new_sock);
 682
 683         release_sock(&sock->sk);
 684         sock_put(&sock->sk);
 685
 686         return;
 687
 688 fail:
 689         /* Send DM */
 690         nfc_llcp_send_dm(local, dsap, ssap, reason);
 691
 692         return;
 693
 694 }
 695
 696 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
 697 {
 698         int nr_frames = 0;
 699         struct nfc_llcp_local *local = sock->local;
 700
 701         pr_debug("Remote ready %d tx queue len %d remote rw %d",
 702                  sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
 703                  sock->rw);
 704
 705         /* Try to queue some I frames for transmission */
 706         while (sock->remote_ready &&
 707                skb_queue_len(&sock->tx_pending_queue) < sock->rw) {
 708                 struct sk_buff *pdu, *pending_pdu;
 709
 710                 pdu = skb_dequeue(&sock->tx_queue);
 711                 if (pdu == NULL)
 712                         break;
 713
 714                 /* Update N(S)/N(R) */
 715                 nfc_llcp_set_nrns(sock, pdu);
 716
 717                 pending_pdu = skb_clone(pdu, GFP_KERNEL);
 718
 719                 skb_queue_tail(&local->tx_queue, pdu);
 720                 skb_queue_tail(&sock->tx_pending_queue, pending_pdu);
 721                 nr_frames++;
 722         }
 723
 724         return nr_frames;
 725 }
 726
 727 static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 728                                struct sk_buff *skb)
 729 {
 730         struct nfc_llcp_sock *llcp_sock;
 731         struct sock *sk;
 732         u8 dsap, ssap, ptype, ns, nr;
 733
 734         ptype = nfc_llcp_ptype(skb);
 735         dsap = nfc_llcp_dsap(skb);
 736         ssap = nfc_llcp_ssap(skb);
 737         ns = nfc_llcp_ns(skb);
 738         nr = nfc_llcp_nr(skb);
 739
 740         pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
 741
 742         llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
 743         if (llcp_sock == NULL) {
 744                 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
 745                 return;
 746         }
 747
 748         sk = &llcp_sock->sk;
 749         lock_sock(sk);
 750         if (sk->sk_state == LLCP_CLOSED) {
 751                 release_sock(sk);
 752                 nfc_llcp_sock_put(llcp_sock);
 753         }
 754
 755         /* Pass the payload upstream */
 756         if (ptype == LLCP_PDU_I) {
 757                 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
 758
 759                 if (ns == llcp_sock->recv_n)
 760                         llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
 761                 else
 762                         pr_err("Received out of sequence I PDU\n");
 763
 764                 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
 765                 if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
 766                         pr_err("receive queue is full\n");
 767                         skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
 768                 }
 769         }
 770
 771         /* Remove skbs from the pending queue */
 772         if (llcp_sock->send_ack_n != nr) {
 773                 struct sk_buff *s, *tmp;
 774
 775                 llcp_sock->send_ack_n = nr;
 776
 777                 skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
 778                         if (nfc_llcp_ns(s) <= nr) {
 779                                 skb_unlink(s, &llcp_sock->tx_pending_queue);
 780                                 kfree_skb(s);
 781                         }
 782         }
 783
 784         if (ptype == LLCP_PDU_RR)
 785                 llcp_sock->remote_ready = true;
 786         else if (ptype == LLCP_PDU_RNR)
 787                 llcp_sock->remote_ready = false;
 788
 789         if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
 790                 nfc_llcp_send_rr(llcp_sock);
 791
 792         release_sock(sk);
 793         nfc_llcp_sock_put(llcp_sock);
 794 }
 795
 796 static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 797                                struct sk_buff *skb)
 798 {
 799         struct nfc_llcp_sock *llcp_sock;
 800         struct sock *sk;
 801         u8 dsap, ssap;
 802
 803         dsap = nfc_llcp_dsap(skb);
 804         ssap = nfc_llcp_ssap(skb);
 805
 806         llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
 807         if (llcp_sock == NULL) {
 808                 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
 809                 return;
 810         }
 811
 812         sk = &llcp_sock->sk;
 813         lock_sock(sk);
 814         if (sk->sk_state == LLCP_CLOSED) {
 815                 release_sock(sk);
 816                 nfc_llcp_sock_put(llcp_sock);
 817         }
 818
 819         if (sk->sk_state == LLCP_CONNECTED) {
 820                 nfc_put_device(local->dev);
 821                 sk->sk_state = LLCP_CLOSED;
 822                 sk->sk_state_change(sk);
 823         }
 824
 825         nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
 826
 827         release_sock(sk);
 828         nfc_llcp_sock_put(llcp_sock);
 829 }
 830
 831 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
 832 {
 833         struct nfc_llcp_sock *llcp_sock;
 834         struct sock *sk;
 835         u8 dsap, ssap;
 836
 837         dsap = nfc_llcp_dsap(skb);
 838         ssap = nfc_llcp_ssap(skb);
 839
 840         llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
 841         if (llcp_sock == NULL) {
 842                 pr_err("Invalid CC\n");
 843                 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
 844
 845                 return;
 846         }
 847
 848         sk = &llcp_sock->sk;
 849
 850         /* Unlink from connecting and link to the client array */
 851         nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
 852         nfc_llcp_sock_link(&local->sockets, sk);
 853         llcp_sock->dsap = ssap;
 854
 855         nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
 856                                       skb->len - LLCP_HEADER_SIZE);
 857
 858         sk->sk_state = LLCP_CONNECTED;
 859         sk->sk_state_change(sk);
 860
 861         nfc_llcp_sock_put(llcp_sock);
 862 }
 863
 864 static void nfc_llcp_rx_work(struct work_struct *work)
 865 {
 866         struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
 867                                                     rx_work);
 868         u8 dsap, ssap, ptype;
 869         struct sk_buff *skb;
 870
 871         skb = local->rx_pending;
 872         if (skb == NULL) {
 873                 pr_debug("No pending SKB\n");
 874                 return;
 875         }
 876
 877         ptype = nfc_llcp_ptype(skb);
 878         dsap = nfc_llcp_dsap(skb);
 879         ssap = nfc_llcp_ssap(skb);
 880
 881         pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
 882
 883         if (ptype != LLCP_PDU_SYMM)
 884                 print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
 885                                16, 1, skb->data, skb->len, true);
 886
 887         switch (ptype) {
 888         case LLCP_PDU_SYMM:
 889                 pr_debug("SYMM\n");
 890                 break;
 891
 892         case LLCP_PDU_CONNECT:
 893                 pr_debug("CONNECT\n");
 894                 nfc_llcp_recv_connect(local, skb);
 895                 break;
 896
 897         case LLCP_PDU_DISC:
 898                 pr_debug("DISC\n");
 899                 nfc_llcp_recv_disc(local, skb);
 900                 break;
 901
 902         case LLCP_PDU_CC:
 903                 pr_debug("CC\n");
 904                 nfc_llcp_recv_cc(local, skb);
 905                 break;
 906
 907         case LLCP_PDU_I:
 908         case LLCP_PDU_RR:
 909         case LLCP_PDU_RNR:
 910                 pr_debug("I frame\n");
 911                 nfc_llcp_recv_hdlc(local, skb);
 912                 break;
 913
 914         }
 915
 916         queue_work(local->tx_wq, &local->tx_work);
 917         kfree_skb(local->rx_pending);
 918         local->rx_pending = NULL;
 919
 920         return;
 921 }
 922
 923 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
 924 {
 925         struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
 926
 927         pr_debug("Received an LLCP PDU\n");
 928         if (err < 0) {
 929                 pr_err("err %d\n", err);
 930                 return;
 931         }
 932
 933         local->rx_pending = skb_get(skb);
 934         del_timer(&local->link_timer);
 935         queue_work(local->rx_wq, &local->rx_work);
 936
 937         return;
 938 }
 939
 940 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
 941 {
 942         struct nfc_llcp_local *local;
 943
 944         local = nfc_llcp_find_local(dev);
 945         if (local == NULL)
 946                 return -ENODEV;
 947
 948         local->rx_pending = skb_get(skb);
 949         del_timer(&local->link_timer);
 950         queue_work(local->rx_wq, &local->rx_work);
 951
 952         return 0;
 953 }
 954
 955 void nfc_llcp_mac_is_down(struct nfc_dev *dev)
 956 {
 957         struct nfc_llcp_local *local;
 958
 959         local = nfc_llcp_find_local(dev);
 960         if (local == NULL)
 961                 return;
 962
 963         nfc_llcp_clear_sdp(local);
 964
 965         /* Close and purge all existing sockets */
 966         nfc_llcp_socket_release(local);
 967 }
 968
 969 void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
 970                         u8 comm_mode, u8 rf_mode)
 971 {
 972         struct nfc_llcp_local *local;
 973
 974         pr_debug("rf mode %d\n", rf_mode);
 975
 976         local = nfc_llcp_find_local(dev);
 977         if (local == NULL)
 978                 return;
 979
 980         local->target_idx = target_idx;
 981         local->comm_mode = comm_mode;
 982         local->rf_mode = rf_mode;
 983
 984         if (rf_mode == NFC_RF_INITIATOR) {
 985                 pr_debug("Queueing Tx work\n");
 986
 987                 queue_work(local->tx_wq, &local->tx_work);
 988         } else {
 989                 mod_timer(&local->link_timer,
 990                           jiffies + msecs_to_jiffies(local->remote_lto));
 991         }
 992 }
 993
 994 int nfc_llcp_register_device(struct nfc_dev *ndev)
 995 {
 996         struct device *dev = &ndev->dev;
 997         struct nfc_llcp_local *local;
 998         char name[32];
 999         int err;
1000
1001         local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
1002         if (local == NULL)
1003                 return -ENOMEM;
1004
1005         local->dev = ndev;
1006         INIT_LIST_HEAD(&local->list);
1007         kref_init(&local->ref);
1008         mutex_init(&local->sdp_lock);
1009         init_timer(&local->link_timer);
1010         local->link_timer.data = (unsigned long) local;
1011         local->link_timer.function = nfc_llcp_symm_timer;
1012
1013         skb_queue_head_init(&local->tx_queue);
1014         INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
1015         snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
1016         local->tx_wq =
1017                 alloc_workqueue(name,
1018                                 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
1019                                 1);
1020         if (local->tx_wq == NULL) {
1021                 err = -ENOMEM;
1022                 goto err_local;
1023         }
1024
1025         local->rx_pending = NULL;
1026         INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
1027         snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
1028         local->rx_wq =
1029                 alloc_workqueue(name,
1030                                 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
1031                                 1);
1032         if (local->rx_wq == NULL) {
1033                 err = -ENOMEM;
1034                 goto err_tx_wq;
1035         }
1036
1037         INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
1038         snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
1039         local->timeout_wq =
1040                 alloc_workqueue(name,
1041                                 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
1042                                 1);
1043         if (local->timeout_wq == NULL) {
1044                 err = -ENOMEM;
1045                 goto err_rx_wq;
1046         }
1047
1048         local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
1049         local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
1050
1051         nfc_llcp_build_gb(local);
1052
1053         local->remote_miu = LLCP_DEFAULT_MIU;
1054         local->remote_lto = LLCP_DEFAULT_LTO;
1055
1056         list_add(&llcp_devices, &local->list);
1057
1058         return 0;
1059
1060 err_rx_wq:
1061         destroy_workqueue(local->rx_wq);
1062
1063 err_tx_wq:
1064         destroy_workqueue(local->tx_wq);
1065
1066 err_local:
1067         kfree(local);
1068
1069         return 0;
1070 }
1071
1072 void nfc_llcp_unregister_device(struct nfc_dev *dev)
1073 {
1074         struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
1075
1076         if (local == NULL) {
1077                 pr_debug("No such device\n");
1078                 return;
1079         }
1080
1081         nfc_llcp_local_put(local);
1082 }
1083
1084 int __init nfc_llcp_init(void)
1085 {
1086         INIT_LIST_HEAD(&llcp_devices);
1087
1088         return nfc_llcp_sock_init();
1089 }
1090
1091 void nfc_llcp_exit(void)
1092 {
1093         nfc_llcp_sock_exit();
1094 }