net/nfc/hci/llc_shdlc.c

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