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