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