net/nfc/hci/shdlc.c

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