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