linux/net/nfc/llcp/llcp.c
<<
>>
Prefs
   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
  30static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
  31
  32static struct list_head llcp_devices;
  33
  34static 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
 100static 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
 108static 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
 117struct 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
 130static char *wks[] = {
 131        NULL,
 132        NULL, /* SDP */
 133        "urn:nfc:sn:ip",
 134        "urn:nfc:sn:obex",
 135        "urn:nfc:sn:snep",
 136};
 137
 138static 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
 160u8 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
 229u8 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
 248void 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
 273u8 *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
 288static 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
 339int 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
 365static 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
 384static u8 nfc_llcp_dsap(struct sk_buff *pdu)
 385{
 386        return (pdu->data[0] & 0xfc) >> 2;
 387}
 388
 389static u8 nfc_llcp_ptype(struct sk_buff *pdu)
 390{
 391        return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
 392}
 393
 394static u8 nfc_llcp_ssap(struct sk_buff *pdu)
 395{
 396        return pdu->data[1] & 0x3f;
 397}
 398
 399static u8 nfc_llcp_ns(struct sk_buff *pdu)
 400{
 401        return pdu->data[2] >> 4;
 402}
 403
 404static u8 nfc_llcp_nr(struct sk_buff *pdu)
 405{
 406        return pdu->data[2] & 0xf;
 407}
 408
 409static 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
 416static 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
 456static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
 457{
 458        sock_put(&sock->sk);
 459}
 460
 461static 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
 484static 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
 563enqueue:
 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
 611fail:
 612        /* Send DM */
 613        nfc_llcp_send_dm(local, dsap, ssap, reason);
 614
 615        return;
 616
 617}
 618
 619static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 620                                struct sk_buff *skb)
 621{
 622        struct nfc_llcp_sock *llcp_sock;
 623        struct sock *sk;
 624        u8 dsap, ssap, ptype, ns, nr;
 625
 626        ptype = nfc_llcp_ptype(skb);
 627        dsap = nfc_llcp_dsap(skb);
 628        ssap = nfc_llcp_ssap(skb);
 629        ns = nfc_llcp_ns(skb);
 630        nr = nfc_llcp_nr(skb);
 631
 632        pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
 633
 634        llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
 635        if (llcp_sock == NULL) {
 636                nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
 637                return;
 638        }
 639
 640        sk = &llcp_sock->sk;
 641        lock_sock(sk);
 642        if (sk->sk_state == LLCP_CLOSED) {
 643                release_sock(sk);
 644                nfc_llcp_sock_put(llcp_sock);
 645        }
 646
 647        if (ns == llcp_sock->recv_n)
 648                llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
 649        else
 650                pr_err("Received out of sequence I PDU\n");
 651
 652        /* Pass the payload upstream */
 653        if (ptype == LLCP_PDU_I) {
 654                pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
 655
 656                skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
 657                if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
 658                        pr_err("receive queue is full\n");
 659                        skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
 660                }
 661        }
 662
 663        /* Remove skbs from the pending queue */
 664        if (llcp_sock->send_ack_n != nr) {
 665                struct sk_buff *s, *tmp;
 666
 667                llcp_sock->send_ack_n = nr;
 668
 669                skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
 670                        if (nfc_llcp_ns(s) <= nr) {
 671                                skb_unlink(s, &llcp_sock->tx_pending_queue);
 672                                kfree_skb(s);
 673                        }
 674        }
 675
 676        /* Queue some I frames for transmission */
 677        while (llcp_sock->remote_ready &&
 678                skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) {
 679                struct sk_buff *pdu, *pending_pdu;
 680
 681                pdu = skb_dequeue(&llcp_sock->tx_queue);
 682                if (pdu == NULL)
 683                        break;
 684
 685                /* Update N(S)/N(R) */
 686                nfc_llcp_set_nrns(llcp_sock, pdu);
 687
 688                pending_pdu = skb_clone(pdu, GFP_KERNEL);
 689
 690                skb_queue_tail(&local->tx_queue, pdu);
 691                skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu);
 692        }
 693
 694        release_sock(sk);
 695        nfc_llcp_sock_put(llcp_sock);
 696}
 697
 698static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 699                                struct sk_buff *skb)
 700{
 701        struct nfc_llcp_sock *llcp_sock;
 702        struct sock *sk;
 703        u8 dsap, ssap;
 704
 705        dsap = nfc_llcp_dsap(skb);
 706        ssap = nfc_llcp_ssap(skb);
 707
 708        llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
 709        if (llcp_sock == NULL) {
 710                nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
 711                return;
 712        }
 713
 714        sk = &llcp_sock->sk;
 715        lock_sock(sk);
 716        if (sk->sk_state == LLCP_CLOSED) {
 717                release_sock(sk);
 718                nfc_llcp_sock_put(llcp_sock);
 719        }
 720
 721
 722        if (sk->sk_state == LLCP_CONNECTED) {
 723                nfc_put_device(local->dev);
 724                sk->sk_state = LLCP_CLOSED;
 725                sk->sk_state_change(sk);
 726        }
 727
 728        nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
 729
 730        release_sock(sk);
 731        nfc_llcp_sock_put(llcp_sock);
 732}
 733
 734static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
 735                                struct sk_buff *skb)
 736{
 737        struct nfc_llcp_sock *llcp_sock;
 738        u8 dsap, ssap;
 739
 740
 741        dsap = nfc_llcp_dsap(skb);
 742        ssap = nfc_llcp_ssap(skb);
 743
 744        llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
 745
 746        if (llcp_sock == NULL)
 747                llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
 748
 749        if (llcp_sock == NULL) {
 750                pr_err("Invalid CC\n");
 751                nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
 752
 753                return;
 754        }
 755
 756        llcp_sock->dsap = ssap;
 757
 758        nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
 759                                skb->len - LLCP_HEADER_SIZE);
 760
 761        nfc_llcp_sock_put(llcp_sock);
 762}
 763
 764static void nfc_llcp_rx_work(struct work_struct *work)
 765{
 766        struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
 767                                                                rx_work);
 768        u8 dsap, ssap, ptype;
 769        struct sk_buff *skb;
 770
 771        skb = local->rx_pending;
 772        if (skb == NULL) {
 773                pr_debug("No pending SKB\n");
 774                return;
 775        }
 776
 777        ptype = nfc_llcp_ptype(skb);
 778        dsap = nfc_llcp_dsap(skb);
 779        ssap = nfc_llcp_ssap(skb);
 780
 781        pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
 782
 783        switch (ptype) {
 784        case LLCP_PDU_SYMM:
 785                pr_debug("SYMM\n");
 786                break;
 787
 788        case LLCP_PDU_CONNECT:
 789                pr_debug("CONNECT\n");
 790                nfc_llcp_recv_connect(local, skb);
 791                break;
 792
 793        case LLCP_PDU_DISC:
 794                pr_debug("DISC\n");
 795                nfc_llcp_recv_disc(local, skb);
 796                break;
 797
 798        case LLCP_PDU_CC:
 799                pr_debug("CC\n");
 800                nfc_llcp_recv_cc(local, skb);
 801                break;
 802
 803        case LLCP_PDU_I:
 804        case LLCP_PDU_RR:
 805                pr_debug("I frame\n");
 806                nfc_llcp_recv_hdlc(local, skb);
 807                break;
 808
 809        }
 810
 811        queue_work(local->tx_wq, &local->tx_work);
 812        kfree_skb(local->rx_pending);
 813        local->rx_pending = NULL;
 814
 815        return;
 816}
 817
 818void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
 819{
 820        struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
 821
 822        pr_debug("Received an LLCP PDU\n");
 823        if (err < 0) {
 824                pr_err("err %d", err);
 825                return;
 826        }
 827
 828        local->rx_pending = skb_get(skb);
 829        del_timer(&local->link_timer);
 830        queue_work(local->rx_wq, &local->rx_work);
 831
 832        return;
 833}
 834
 835void nfc_llcp_mac_is_down(struct nfc_dev *dev)
 836{
 837        struct nfc_llcp_local *local;
 838
 839        local = nfc_llcp_find_local(dev);
 840        if (local == NULL)
 841                return;
 842
 843        /* Close and purge all existing sockets */
 844        nfc_llcp_socket_release(local);
 845}
 846
 847void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
 848                        u8 comm_mode, u8 rf_mode)
 849{
 850        struct nfc_llcp_local *local;
 851
 852        pr_debug("rf mode %d\n", rf_mode);
 853
 854        local = nfc_llcp_find_local(dev);
 855        if (local == NULL)
 856                return;
 857
 858        local->target_idx = target_idx;
 859        local->comm_mode = comm_mode;
 860        local->rf_mode = rf_mode;
 861
 862        if (rf_mode == NFC_RF_INITIATOR) {
 863                pr_debug("Queueing Tx work\n");
 864
 865                queue_work(local->tx_wq, &local->tx_work);
 866        } else {
 867                mod_timer(&local->link_timer,
 868                        jiffies + msecs_to_jiffies(local->remote_lto));
 869        }
 870}
 871
 872int nfc_llcp_register_device(struct nfc_dev *ndev)
 873{
 874        struct device *dev = &ndev->dev;
 875        struct nfc_llcp_local *local;
 876        char name[32];
 877        int err;
 878
 879        local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
 880        if (local == NULL)
 881                return -ENOMEM;
 882
 883        local->dev = ndev;
 884        INIT_LIST_HEAD(&local->list);
 885        mutex_init(&local->sdp_lock);
 886        mutex_init(&local->socket_lock);
 887        init_timer(&local->link_timer);
 888        local->link_timer.data = (unsigned long) local;
 889        local->link_timer.function = nfc_llcp_symm_timer;
 890
 891        skb_queue_head_init(&local->tx_queue);
 892        INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
 893        snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
 894        local->tx_wq = alloc_workqueue(name,
 895                        WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
 896        if (local->tx_wq == NULL) {
 897                err = -ENOMEM;
 898                goto err_local;
 899        }
 900
 901        local->rx_pending = NULL;
 902        INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
 903        snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
 904        local->rx_wq = alloc_workqueue(name,
 905                        WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
 906        if (local->rx_wq == NULL) {
 907                err = -ENOMEM;
 908                goto err_tx_wq;
 909        }
 910
 911        INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
 912        snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
 913        local->timeout_wq = alloc_workqueue(name,
 914                        WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
 915        if (local->timeout_wq == NULL) {
 916                err = -ENOMEM;
 917                goto err_rx_wq;
 918        }
 919
 920        nfc_llcp_build_gb(local);
 921
 922        local->remote_miu = LLCP_DEFAULT_MIU;
 923        local->remote_lto = LLCP_DEFAULT_LTO;
 924        local->remote_rw = LLCP_DEFAULT_RW;
 925
 926        list_add(&llcp_devices, &local->list);
 927
 928        return 0;
 929
 930err_rx_wq:
 931        destroy_workqueue(local->rx_wq);
 932
 933err_tx_wq:
 934        destroy_workqueue(local->tx_wq);
 935
 936err_local:
 937        kfree(local);
 938
 939        return 0;
 940}
 941
 942void nfc_llcp_unregister_device(struct nfc_dev *dev)
 943{
 944        struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
 945
 946        if (local == NULL) {
 947                pr_debug("No such device\n");
 948                return;
 949        }
 950
 951        list_del(&local->list);
 952        nfc_llcp_socket_release(local);
 953        del_timer_sync(&local->link_timer);
 954        skb_queue_purge(&local->tx_queue);
 955        destroy_workqueue(local->tx_wq);
 956        destroy_workqueue(local->rx_wq);
 957        kfree_skb(local->rx_pending);
 958        kfree(local);
 959}
 960
 961int __init nfc_llcp_init(void)
 962{
 963        INIT_LIST_HEAD(&llcp_devices);
 964
 965        return nfc_llcp_sock_init();
 966}
 967
 968void nfc_llcp_exit(void)
 969{
 970        nfc_llcp_sock_exit();
 971}
 972