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