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