linux/net/nfc/hci/llc_shdlc.c
<<
>>
Prefs
   1/*
   2 * shdlc Link Layer Control
   3 *
   4 * Copyright (C) 2012  Intel Corporation. All rights reserved.
   5 *
   6 * This program is free software; you can redistribute it and/or modify it
   7 * under the terms and conditions of the GNU General Public License,
   8 * version 2, as published by the Free Software Foundation.
   9 *
  10 * This program is distributed in the hope that it will be useful,
  11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13 * GNU General Public License for more details.
  14 *
  15 * You should have received a copy of the GNU General Public License
  16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
  17 */
  18
  19#define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
  20
  21#include <linux/types.h>
  22#include <linux/sched.h>
  23#include <linux/wait.h>
  24#include <linux/slab.h>
  25#include <linux/skbuff.h>
  26
  27#include "llc.h"
  28
  29enum shdlc_state {
  30        SHDLC_DISCONNECTED = 0,
  31        SHDLC_CONNECTING = 1,
  32        SHDLC_NEGOTIATING = 2,
  33        SHDLC_HALF_CONNECTED = 3,
  34        SHDLC_CONNECTED = 4
  35};
  36
  37struct llc_shdlc {
  38        struct nfc_hci_dev *hdev;
  39        xmit_to_drv_t xmit_to_drv;
  40        rcv_to_hci_t rcv_to_hci;
  41
  42        struct mutex state_mutex;
  43        enum shdlc_state state;
  44        int hard_fault;
  45
  46        wait_queue_head_t *connect_wq;
  47        int connect_tries;
  48        int connect_result;
  49        struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
  50
  51        u8 w;                           /* window size */
  52        bool srej_support;
  53
  54        struct timer_list t1_timer;     /* send ack timeout */
  55        bool t1_active;
  56
  57        struct timer_list t2_timer;     /* guard/retransmit timeout */
  58        bool t2_active;
  59
  60        int ns;                         /* next seq num for send */
  61        int nr;                         /* next expected seq num for receive */
  62        int dnr;                        /* oldest sent unacked seq num */
  63
  64        struct sk_buff_head rcv_q;
  65
  66        struct sk_buff_head send_q;
  67        bool rnr;                       /* other side is not ready to receive */
  68
  69        struct sk_buff_head ack_pending_q;
  70
  71        struct work_struct sm_work;
  72
  73        int tx_headroom;
  74        int tx_tailroom;
  75
  76        llc_failure_t llc_failure;
  77};
  78
  79#define SHDLC_LLC_HEAD_ROOM     2
  80
  81#define SHDLC_MAX_WINDOW        4
  82#define SHDLC_SREJ_SUPPORT      false
  83
  84#define SHDLC_CONTROL_HEAD_MASK 0xe0
  85#define SHDLC_CONTROL_HEAD_I    0x80
  86#define SHDLC_CONTROL_HEAD_I2   0xa0
  87#define SHDLC_CONTROL_HEAD_S    0xc0
  88#define SHDLC_CONTROL_HEAD_U    0xe0
  89
  90#define SHDLC_CONTROL_NS_MASK   0x38
  91#define SHDLC_CONTROL_NR_MASK   0x07
  92#define SHDLC_CONTROL_TYPE_MASK 0x18
  93
  94#define SHDLC_CONTROL_M_MASK    0x1f
  95
  96enum sframe_type {
  97        S_FRAME_RR = 0x00,
  98        S_FRAME_REJ = 0x01,
  99        S_FRAME_RNR = 0x02,
 100        S_FRAME_SREJ = 0x03
 101};
 102
 103enum uframe_modifier {
 104        U_FRAME_UA = 0x06,
 105        U_FRAME_RSET = 0x19
 106};
 107
 108#define SHDLC_CONNECT_VALUE_MS  5
 109#define SHDLC_T1_VALUE_MS(w)    ((5 * w) / 4)
 110#define SHDLC_T2_VALUE_MS       300
 111
 112#define SHDLC_DUMP_SKB(info, skb)                                 \
 113do {                                                              \
 114        pr_debug("%s:\n", info);                                  \
 115        print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
 116                       16, 1, skb->data, skb->len, 0);            \
 117} while (0)
 118
 119/* checks x < y <= z modulo 8 */
 120static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
 121{
 122        if (x < z)
 123                return ((x < y) && (y <= z)) ? true : false;
 124        else
 125                return ((y > x) || (y <= z)) ? true : false;
 126}
 127
 128/* checks x <= y < z modulo 8 */
 129static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
 130{
 131        if (x <= z)
 132                return ((x <= y) && (y < z)) ? true : false;
 133        else                    /* x > z -> z+8 > x */
 134                return ((y >= x) || (y < z)) ? true : false;
 135}
 136
 137static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc,
 138                                           int payload_len)
 139{
 140        struct sk_buff *skb;
 141
 142        skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
 143                        shdlc->tx_tailroom + payload_len, GFP_KERNEL);
 144        if (skb)
 145                skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
 146
 147        return skb;
 148}
 149
 150/* immediately sends an S frame. */
 151static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc,
 152                                  enum sframe_type sframe_type, int nr)
 153{
 154        int r;
 155        struct sk_buff *skb;
 156
 157        pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
 158
 159        skb = llc_shdlc_alloc_skb(shdlc, 0);
 160        if (skb == NULL)
 161                return -ENOMEM;
 162
 163        *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
 164
 165        r = shdlc->xmit_to_drv(shdlc->hdev, skb);
 166
 167        kfree_skb(skb);
 168
 169        return r;
 170}
 171
 172/* immediately sends an U frame. skb may contain optional payload */
 173static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc,
 174                                  struct sk_buff *skb,
 175                                  enum uframe_modifier uframe_modifier)
 176{
 177        int r;
 178
 179        pr_debug("uframe_modifier=%d\n", uframe_modifier);
 180
 181        *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
 182
 183        r = shdlc->xmit_to_drv(shdlc->hdev, skb);
 184
 185        kfree_skb(skb);
 186
 187        return r;
 188}
 189
 190/*
 191 * Free ack_pending frames until y_nr - 1, and reset t2 according to
 192 * the remaining oldest ack_pending frame sent time
 193 */
 194static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
 195{
 196        struct sk_buff *skb;
 197        int dnr = shdlc->dnr;   /* MUST initially be < y_nr */
 198
 199        pr_debug("release ack pending up to frame %d excluded\n", y_nr);
 200
 201        while (dnr != y_nr) {
 202                pr_debug("release ack pending frame %d\n", dnr);
 203
 204                skb = skb_dequeue(&shdlc->ack_pending_q);
 205                kfree_skb(skb);
 206
 207                dnr = (dnr + 1) % 8;
 208        }
 209
 210        if (skb_queue_empty(&shdlc->ack_pending_q)) {
 211                if (shdlc->t2_active) {
 212                        del_timer_sync(&shdlc->t2_timer);
 213                        shdlc->t2_active = false;
 214
 215                        pr_debug
 216                            ("All sent frames acked. Stopped T2(retransmit)\n");
 217                }
 218        } else {
 219                skb = skb_peek(&shdlc->ack_pending_q);
 220
 221                mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
 222                          msecs_to_jiffies(SHDLC_T2_VALUE_MS));
 223                shdlc->t2_active = true;
 224
 225                pr_debug
 226                    ("Start T2(retransmit) for remaining unacked sent frames\n");
 227        }
 228}
 229
 230/*
 231 * Receive validated frames from lower layer. skb contains HCI payload only.
 232 * Handle according to algorithm at spec:10.8.2
 233 */
 234static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
 235                                  struct sk_buff *skb, int ns, int nr)
 236{
 237        int x_ns = ns;
 238        int y_nr = nr;
 239
 240        pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
 241
 242        if (shdlc->state != SHDLC_CONNECTED)
 243                goto exit;
 244
 245        if (x_ns != shdlc->nr) {
 246                llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
 247                goto exit;
 248        }
 249
 250        if (shdlc->t1_active == false) {
 251                shdlc->t1_active = true;
 252                mod_timer(&shdlc->t1_timer, jiffies +
 253                          msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
 254                pr_debug("(re)Start T1(send ack)\n");
 255        }
 256
 257        if (skb->len) {
 258                shdlc->rcv_to_hci(shdlc->hdev, skb);
 259                skb = NULL;
 260        }
 261
 262        shdlc->nr = (shdlc->nr + 1) % 8;
 263
 264        if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
 265                llc_shdlc_reset_t2(shdlc, y_nr);
 266
 267                shdlc->dnr = y_nr;
 268        }
 269
 270exit:
 271        kfree_skb(skb);
 272}
 273
 274static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
 275{
 276        pr_debug("remote acked up to frame %d excluded\n", y_nr);
 277
 278        if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
 279                llc_shdlc_reset_t2(shdlc, y_nr);
 280                shdlc->dnr = y_nr;
 281        }
 282}
 283
 284static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
 285{
 286        struct sk_buff *skb;
 287
 288        pr_debug("ns reset to %d\n", shdlc->dnr);
 289
 290        while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
 291                skb_pull(skb, 1);       /* remove control field */
 292                skb_queue_head(&shdlc->send_q, skb);
 293        }
 294        shdlc->ns = shdlc->dnr;
 295}
 296
 297static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
 298{
 299        struct sk_buff *skb;
 300
 301        pr_debug("remote asks retransmission from frame %d\n", y_nr);
 302
 303        if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
 304                if (shdlc->t2_active) {
 305                        del_timer_sync(&shdlc->t2_timer);
 306                        shdlc->t2_active = false;
 307                        pr_debug("Stopped T2(retransmit)\n");
 308                }
 309
 310                if (shdlc->dnr != y_nr) {
 311                        while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
 312                                skb = skb_dequeue(&shdlc->ack_pending_q);
 313                                kfree_skb(skb);
 314                        }
 315                }
 316
 317                llc_shdlc_requeue_ack_pending(shdlc);
 318        }
 319}
 320
 321/* See spec RR:10.8.3 REJ:10.8.4 */
 322static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,
 323                                  enum sframe_type s_frame_type, int nr)
 324{
 325        struct sk_buff *skb;
 326
 327        if (shdlc->state != SHDLC_CONNECTED)
 328                return;
 329
 330        switch (s_frame_type) {
 331        case S_FRAME_RR:
 332                llc_shdlc_rcv_ack(shdlc, nr);
 333                if (shdlc->rnr == true) {       /* see SHDLC 10.7.7 */
 334                        shdlc->rnr = false;
 335                        if (shdlc->send_q.qlen == 0) {
 336                                skb = llc_shdlc_alloc_skb(shdlc, 0);
 337                                if (skb)
 338                                        skb_queue_tail(&shdlc->send_q, skb);
 339                        }
 340                }
 341                break;
 342        case S_FRAME_REJ:
 343                llc_shdlc_rcv_rej(shdlc, nr);
 344                break;
 345        case S_FRAME_RNR:
 346                llc_shdlc_rcv_ack(shdlc, nr);
 347                shdlc->rnr = true;
 348                break;
 349        default:
 350                break;
 351        }
 352}
 353
 354static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
 355{
 356        pr_debug("result=%d\n", r);
 357
 358        del_timer_sync(&shdlc->connect_timer);
 359
 360        if (r == 0) {
 361                shdlc->ns = 0;
 362                shdlc->nr = 0;
 363                shdlc->dnr = 0;
 364
 365                shdlc->state = SHDLC_HALF_CONNECTED;
 366        } else {
 367                shdlc->state = SHDLC_DISCONNECTED;
 368        }
 369
 370        shdlc->connect_result = r;
 371
 372        wake_up(shdlc->connect_wq);
 373}
 374
 375static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)
 376{
 377        struct sk_buff *skb;
 378
 379        pr_debug("\n");
 380
 381        skb = llc_shdlc_alloc_skb(shdlc, 2);
 382        if (skb == NULL)
 383                return -ENOMEM;
 384
 385        *skb_put(skb, 1) = SHDLC_MAX_WINDOW;
 386        *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0;
 387
 388        return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
 389}
 390
 391static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)
 392{
 393        struct sk_buff *skb;
 394
 395        pr_debug("\n");
 396
 397        skb = llc_shdlc_alloc_skb(shdlc, 0);
 398        if (skb == NULL)
 399                return -ENOMEM;
 400
 401        return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
 402}
 403
 404static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
 405                                  struct sk_buff *skb,
 406                                  enum uframe_modifier u_frame_modifier)
 407{
 408        u8 w = SHDLC_MAX_WINDOW;
 409        bool srej_support = SHDLC_SREJ_SUPPORT;
 410        int r;
 411
 412        pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
 413
 414        switch (u_frame_modifier) {
 415        case U_FRAME_RSET:
 416                switch (shdlc->state) {
 417                case SHDLC_NEGOTIATING:
 418                case SHDLC_CONNECTING:
 419                        /*
 420                         * We sent RSET, but chip wants to negociate or we
 421                         * got RSET before we managed to send out our.
 422                         */
 423                        if (skb->len > 0)
 424                                w = skb->data[0];
 425
 426                        if (skb->len > 1)
 427                                srej_support = skb->data[1] & 0x01 ? true :
 428                                               false;
 429
 430                        if ((w <= SHDLC_MAX_WINDOW) &&
 431                            (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
 432                                shdlc->w = w;
 433                                shdlc->srej_support = srej_support;
 434                                r = llc_shdlc_connect_send_ua(shdlc);
 435                                llc_shdlc_connect_complete(shdlc, r);
 436                        }
 437                        break;
 438                case SHDLC_HALF_CONNECTED:
 439                        /*
 440                         * Chip resent RSET due to its timeout - Ignote it
 441                         * as we already sent UA.
 442                         */
 443                        break;
 444                case SHDLC_CONNECTED:
 445                        /*
 446                         * Chip wants to reset link. This is unexpected and
 447                         * unsupported.
 448                         */
 449                        shdlc->hard_fault = -ECONNRESET;
 450                        break;
 451                default:
 452                        break;
 453                }
 454                break;
 455        case U_FRAME_UA:
 456                if ((shdlc->state == SHDLC_CONNECTING &&
 457                     shdlc->connect_tries > 0) ||
 458                    (shdlc->state == SHDLC_NEGOTIATING)) {
 459                        llc_shdlc_connect_complete(shdlc, 0);
 460                        shdlc->state = SHDLC_CONNECTED;
 461                }
 462                break;
 463        default:
 464                break;
 465        }
 466
 467        kfree_skb(skb);
 468}
 469
 470static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
 471{
 472        struct sk_buff *skb;
 473        u8 control;
 474        int nr;
 475        int ns;
 476        enum sframe_type s_frame_type;
 477        enum uframe_modifier u_frame_modifier;
 478
 479        if (shdlc->rcv_q.qlen)
 480                pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
 481
 482        while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
 483                control = skb->data[0];
 484                skb_pull(skb, 1);
 485                switch (control & SHDLC_CONTROL_HEAD_MASK) {
 486                case SHDLC_CONTROL_HEAD_I:
 487                case SHDLC_CONTROL_HEAD_I2:
 488                        if (shdlc->state == SHDLC_HALF_CONNECTED)
 489                                shdlc->state = SHDLC_CONNECTED;
 490
 491                        ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
 492                        nr = control & SHDLC_CONTROL_NR_MASK;
 493                        llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
 494                        break;
 495                case SHDLC_CONTROL_HEAD_S:
 496                        if (shdlc->state == SHDLC_HALF_CONNECTED)
 497                                shdlc->state = SHDLC_CONNECTED;
 498
 499                        s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
 500                        nr = control & SHDLC_CONTROL_NR_MASK;
 501                        llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
 502                        kfree_skb(skb);
 503                        break;
 504                case SHDLC_CONTROL_HEAD_U:
 505                        u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
 506                        llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
 507                        break;
 508                default:
 509                        pr_err("UNKNOWN Control=%d\n", control);
 510                        kfree_skb(skb);
 511                        break;
 512                }
 513        }
 514}
 515
 516static int llc_shdlc_w_used(int ns, int dnr)
 517{
 518        int unack_count;
 519
 520        if (dnr <= ns)
 521                unack_count = ns - dnr;
 522        else
 523                unack_count = 8 - dnr + ns;
 524
 525        return unack_count;
 526}
 527
 528/* Send frames according to algorithm at spec:10.8.1 */
 529static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
 530{
 531        struct sk_buff *skb;
 532        int r;
 533        unsigned long time_sent;
 534
 535        if (shdlc->send_q.qlen)
 536                pr_debug
 537                    ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
 538                     shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
 539                     shdlc->rnr == false ? "false" : "true",
 540                     shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
 541                     shdlc->ack_pending_q.qlen);
 542
 543        while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
 544               (shdlc->rnr == false)) {
 545
 546                if (shdlc->t1_active) {
 547                        del_timer_sync(&shdlc->t1_timer);
 548                        shdlc->t1_active = false;
 549                        pr_debug("Stopped T1(send ack)\n");
 550                }
 551
 552                skb = skb_dequeue(&shdlc->send_q);
 553
 554                *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
 555                                    shdlc->nr;
 556
 557                pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
 558                         shdlc->nr);
 559                SHDLC_DUMP_SKB("shdlc frame written", skb);
 560
 561                r = shdlc->xmit_to_drv(shdlc->hdev, skb);
 562                if (r < 0) {
 563                        shdlc->hard_fault = r;
 564                        break;
 565                }
 566
 567                shdlc->ns = (shdlc->ns + 1) % 8;
 568
 569                time_sent = jiffies;
 570                *(unsigned long *)skb->cb = time_sent;
 571
 572                skb_queue_tail(&shdlc->ack_pending_q, skb);
 573
 574                if (shdlc->t2_active == false) {
 575                        shdlc->t2_active = true;
 576                        mod_timer(&shdlc->t2_timer, time_sent +
 577                                  msecs_to_jiffies(SHDLC_T2_VALUE_MS));
 578                        pr_debug("Started T2 (retransmit)\n");
 579                }
 580        }
 581}
 582
 583static void llc_shdlc_connect_timeout(unsigned long data)
 584{
 585        struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
 586
 587        pr_debug("\n");
 588
 589        schedule_work(&shdlc->sm_work);
 590}
 591
 592static void llc_shdlc_t1_timeout(unsigned long data)
 593{
 594        struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
 595
 596        pr_debug("SoftIRQ: need to send ack\n");
 597
 598        schedule_work(&shdlc->sm_work);
 599}
 600
 601static void llc_shdlc_t2_timeout(unsigned long data)
 602{
 603        struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
 604
 605        pr_debug("SoftIRQ: need to retransmit\n");
 606
 607        schedule_work(&shdlc->sm_work);
 608}
 609
 610static void llc_shdlc_sm_work(struct work_struct *work)
 611{
 612        struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
 613        int r;
 614
 615        pr_debug("\n");
 616
 617        mutex_lock(&shdlc->state_mutex);
 618
 619        switch (shdlc->state) {
 620        case SHDLC_DISCONNECTED:
 621                skb_queue_purge(&shdlc->rcv_q);
 622                skb_queue_purge(&shdlc->send_q);
 623                skb_queue_purge(&shdlc->ack_pending_q);
 624                break;
 625        case SHDLC_CONNECTING:
 626                if (shdlc->hard_fault) {
 627                        llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
 628                        break;
 629                }
 630
 631                if (shdlc->connect_tries++ < 5)
 632                        r = llc_shdlc_connect_initiate(shdlc);
 633                else
 634                        r = -ETIME;
 635                if (r < 0) {
 636                        llc_shdlc_connect_complete(shdlc, r);
 637                } else {
 638                        mod_timer(&shdlc->connect_timer, jiffies +
 639                                  msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
 640
 641                        shdlc->state = SHDLC_NEGOTIATING;
 642                }
 643                break;
 644        case SHDLC_NEGOTIATING:
 645                if (timer_pending(&shdlc->connect_timer) == 0) {
 646                        shdlc->state = SHDLC_CONNECTING;
 647                        schedule_work(&shdlc->sm_work);
 648                }
 649
 650                llc_shdlc_handle_rcv_queue(shdlc);
 651
 652                if (shdlc->hard_fault) {
 653                        llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
 654                        break;
 655                }
 656                break;
 657        case SHDLC_HALF_CONNECTED:
 658        case SHDLC_CONNECTED:
 659                llc_shdlc_handle_rcv_queue(shdlc);
 660                llc_shdlc_handle_send_queue(shdlc);
 661
 662                if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
 663                        pr_debug
 664                            ("Handle T1(send ack) elapsed (T1 now inactive)\n");
 665
 666                        shdlc->t1_active = false;
 667                        r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
 668                                                   shdlc->nr);
 669                        if (r < 0)
 670                                shdlc->hard_fault = r;
 671                }
 672
 673                if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
 674                        pr_debug
 675                            ("Handle T2(retransmit) elapsed (T2 inactive)\n");
 676
 677                        shdlc->t2_active = false;
 678
 679                        llc_shdlc_requeue_ack_pending(shdlc);
 680                        llc_shdlc_handle_send_queue(shdlc);
 681                }
 682
 683                if (shdlc->hard_fault)
 684                        shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
 685                break;
 686        default:
 687                break;
 688        }
 689        mutex_unlock(&shdlc->state_mutex);
 690}
 691
 692/*
 693 * Called from syscall context to establish shdlc link. Sleeps until
 694 * link is ready or failure.
 695 */
 696static int llc_shdlc_connect(struct llc_shdlc *shdlc)
 697{
 698        DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
 699
 700        pr_debug("\n");
 701
 702        mutex_lock(&shdlc->state_mutex);
 703
 704        shdlc->state = SHDLC_CONNECTING;
 705        shdlc->connect_wq = &connect_wq;
 706        shdlc->connect_tries = 0;
 707        shdlc->connect_result = 1;
 708
 709        mutex_unlock(&shdlc->state_mutex);
 710
 711        schedule_work(&shdlc->sm_work);
 712
 713        wait_event(connect_wq, shdlc->connect_result != 1);
 714
 715        return shdlc->connect_result;
 716}
 717
 718static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
 719{
 720        pr_debug("\n");
 721
 722        mutex_lock(&shdlc->state_mutex);
 723
 724        shdlc->state = SHDLC_DISCONNECTED;
 725
 726        mutex_unlock(&shdlc->state_mutex);
 727
 728        schedule_work(&shdlc->sm_work);
 729}
 730
 731/*
 732 * Receive an incoming shdlc frame. Frame has already been crc-validated.
 733 * skb contains only LLC header and payload.
 734 * If skb == NULL, it is a notification that the link below is dead.
 735 */
 736static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
 737{
 738        if (skb == NULL) {
 739                pr_err("NULL Frame -> link is dead\n");
 740                shdlc->hard_fault = -EREMOTEIO;
 741        } else {
 742                SHDLC_DUMP_SKB("incoming frame", skb);
 743                skb_queue_tail(&shdlc->rcv_q, skb);
 744        }
 745
 746        schedule_work(&shdlc->sm_work);
 747}
 748
 749static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
 750                            rcv_to_hci_t rcv_to_hci, int tx_headroom,
 751                            int tx_tailroom, int *rx_headroom, int *rx_tailroom,
 752                            llc_failure_t llc_failure)
 753{
 754        struct llc_shdlc *shdlc;
 755
 756        *rx_headroom = SHDLC_LLC_HEAD_ROOM;
 757        *rx_tailroom = 0;
 758
 759        shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
 760        if (shdlc == NULL)
 761                return NULL;
 762
 763        mutex_init(&shdlc->state_mutex);
 764        shdlc->state = SHDLC_DISCONNECTED;
 765
 766        init_timer(&shdlc->connect_timer);
 767        shdlc->connect_timer.data = (unsigned long)shdlc;
 768        shdlc->connect_timer.function = llc_shdlc_connect_timeout;
 769
 770        init_timer(&shdlc->t1_timer);
 771        shdlc->t1_timer.data = (unsigned long)shdlc;
 772        shdlc->t1_timer.function = llc_shdlc_t1_timeout;
 773
 774        init_timer(&shdlc->t2_timer);
 775        shdlc->t2_timer.data = (unsigned long)shdlc;
 776        shdlc->t2_timer.function = llc_shdlc_t2_timeout;
 777
 778        shdlc->w = SHDLC_MAX_WINDOW;
 779        shdlc->srej_support = SHDLC_SREJ_SUPPORT;
 780
 781        skb_queue_head_init(&shdlc->rcv_q);
 782        skb_queue_head_init(&shdlc->send_q);
 783        skb_queue_head_init(&shdlc->ack_pending_q);
 784
 785        INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
 786
 787        shdlc->hdev = hdev;
 788        shdlc->xmit_to_drv = xmit_to_drv;
 789        shdlc->rcv_to_hci = rcv_to_hci;
 790        shdlc->tx_headroom = tx_headroom;
 791        shdlc->tx_tailroom = tx_tailroom;
 792        shdlc->llc_failure = llc_failure;
 793
 794        return shdlc;
 795}
 796
 797static void llc_shdlc_deinit(struct nfc_llc *llc)
 798{
 799        struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
 800
 801        skb_queue_purge(&shdlc->rcv_q);
 802        skb_queue_purge(&shdlc->send_q);
 803        skb_queue_purge(&shdlc->ack_pending_q);
 804
 805        kfree(shdlc);
 806}
 807
 808static int llc_shdlc_start(struct nfc_llc *llc)
 809{
 810        struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
 811
 812        return llc_shdlc_connect(shdlc);
 813}
 814
 815static int llc_shdlc_stop(struct nfc_llc *llc)
 816{
 817        struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
 818
 819        llc_shdlc_disconnect(shdlc);
 820
 821        return 0;
 822}
 823
 824static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
 825{
 826        struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
 827
 828        llc_shdlc_recv_frame(shdlc, skb);
 829}
 830
 831static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
 832{
 833        struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
 834
 835        skb_queue_tail(&shdlc->send_q, skb);
 836
 837        schedule_work(&shdlc->sm_work);
 838
 839        return 0;
 840}
 841
 842static struct nfc_llc_ops llc_shdlc_ops = {
 843        .init = llc_shdlc_init,
 844        .deinit = llc_shdlc_deinit,
 845        .start = llc_shdlc_start,
 846        .stop = llc_shdlc_stop,
 847        .rcv_from_drv = llc_shdlc_rcv_from_drv,
 848        .xmit_from_hci = llc_shdlc_xmit_from_hci,
 849};
 850
 851int nfc_llc_shdlc_register(void)
 852{
 853        return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
 854}
 855