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