kernel/net/nfc/hci/llc_shdlc.c

   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
  29 enum 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
  37 struct 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
  96 enum sframe_type {
  97         S_FRAME_RR = 0x00,
  98         S_FRAME_REJ = 0x01,
  99         S_FRAME_RNR = 0x02,
 100         S_FRAME_SREJ = 0x03
 101 };
 102
 103 enum 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)                                 \
 113 do {                                                              \
 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 */
 120 static 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 */
 129 static 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
 137 static 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. */
 151 static 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 */
 173 static 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  */
 194 static 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  */
 234 static 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
 270 exit:
 271         kfree_skb(skb);
 272 }
 273
 274 static 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
 284 static 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
 297 static 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 */
 322 static 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
 354 static 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
 375 static 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
 391 static 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
 404 static 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
 470 static 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
 516 static 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 */
 529 static 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
 583 static 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
 592 static 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
 601 static 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
 610 static 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  */
 696 static 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
 718 static 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  */
 736 static 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
 749 static 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
 797 static 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
 808 static 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
 815 static 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
 824 static 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
 831 static 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
 842 static 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
 851 int nfc_llc_shdlc_register(void)
 852 {
 853         return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
 854 }