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(const 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(const 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(const 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("All sent frames acked. Stopped T2(retransmit)\n"); 205 } 206 } else { 207 skb = skb_peek(&shdlc->ack_pending_q); 208 209 mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb + 210 msecs_to_jiffies(SHDLC_T2_VALUE_MS)); 211 shdlc->t2_active = true; 212 213 pr_debug("Start T2(retransmit) for remaining unacked sent frames\n"); 214 } 215 } 216 217 /* 218 * Receive validated frames from lower layer. skb contains HCI payload only. 219 * Handle according to algorithm at spec:10.8.2 220 */ 221 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc, 222 struct sk_buff *skb, int ns, int nr) 223 { 224 int x_ns = ns; 225 int y_nr = nr; 226 227 pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr); 228 229 if (shdlc->state != SHDLC_CONNECTED) 230 goto exit; 231 232 if (x_ns != shdlc->nr) { 233 llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr); 234 goto exit; 235 } 236 237 if (!shdlc->t1_active) { 238 shdlc->t1_active = true; 239 mod_timer(&shdlc->t1_timer, jiffies + 240 msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w))); 241 pr_debug("(re)Start T1(send ack)\n"); 242 } 243 244 if (skb->len) { 245 shdlc->rcv_to_hci(shdlc->hdev, skb); 246 skb = NULL; 247 } 248 249 shdlc->nr = (shdlc->nr + 1) % 8; 250 251 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { 252 llc_shdlc_reset_t2(shdlc, y_nr); 253 254 shdlc->dnr = y_nr; 255 } 256 257 exit: 258 kfree_skb(skb); 259 } 260 261 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr) 262 { 263 pr_debug("remote acked up to frame %d excluded\n", y_nr); 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 shdlc->dnr = y_nr; 268 } 269 } 270 271 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc) 272 { 273 struct sk_buff *skb; 274 275 pr_debug("ns reset to %d\n", shdlc->dnr); 276 277 while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) { 278 skb_pull(skb, 1); /* remove control field */ 279 skb_queue_head(&shdlc->send_q, skb); 280 } 281 shdlc->ns = shdlc->dnr; 282 } 283 284 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr) 285 { 286 struct sk_buff *skb; 287 288 pr_debug("remote asks retransmission from frame %d\n", y_nr); 289 290 if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) { 291 if (shdlc->t2_active) { 292 del_timer_sync(&shdlc->t2_timer); 293 shdlc->t2_active = false; 294 pr_debug("Stopped T2(retransmit)\n"); 295 } 296 297 if (shdlc->dnr != y_nr) { 298 while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) { 299 skb = skb_dequeue(&shdlc->ack_pending_q); 300 kfree_skb(skb); 301 } 302 } 303 304 llc_shdlc_requeue_ack_pending(shdlc); 305 } 306 } 307 308 /* See spec RR:10.8.3 REJ:10.8.4 */ 309 static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc, 310 enum sframe_type s_frame_type, int nr) 311 { 312 struct sk_buff *skb; 313 314 if (shdlc->state != SHDLC_CONNECTED) 315 return; 316 317 switch (s_frame_type) { 318 case S_FRAME_RR: 319 llc_shdlc_rcv_ack(shdlc, nr); 320 if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */ 321 shdlc->rnr = false; 322 if (shdlc->send_q.qlen == 0) { 323 skb = llc_shdlc_alloc_skb(shdlc, 0); 324 if (skb) 325 skb_queue_tail(&shdlc->send_q, skb); 326 } 327 } 328 break; 329 case S_FRAME_REJ: 330 llc_shdlc_rcv_rej(shdlc, nr); 331 break; 332 case S_FRAME_RNR: 333 llc_shdlc_rcv_ack(shdlc, nr); 334 shdlc->rnr = true; 335 break; 336 default: 337 break; 338 } 339 } 340 341 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r) 342 { 343 pr_debug("result=%d\n", r); 344 345 del_timer_sync(&shdlc->connect_timer); 346 347 if (r == 0) { 348 shdlc->ns = 0; 349 shdlc->nr = 0; 350 shdlc->dnr = 0; 351 352 shdlc->state = SHDLC_HALF_CONNECTED; 353 } else { 354 shdlc->state = SHDLC_DISCONNECTED; 355 } 356 357 shdlc->connect_result = r; 358 359 wake_up(shdlc->connect_wq); 360 } 361 362 static int llc_shdlc_connect_initiate(const struct llc_shdlc *shdlc) 363 { 364 struct sk_buff *skb; 365 366 pr_debug("\n"); 367 368 skb = llc_shdlc_alloc_skb(shdlc, 2); 369 if (skb == NULL) 370 return -ENOMEM; 371 372 skb_put_u8(skb, SHDLC_MAX_WINDOW); 373 skb_put_u8(skb, SHDLC_SREJ_SUPPORT ? 1 : 0); 374 375 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET); 376 } 377 378 static int llc_shdlc_connect_send_ua(const struct llc_shdlc *shdlc) 379 { 380 struct sk_buff *skb; 381 382 pr_debug("\n"); 383 384 skb = llc_shdlc_alloc_skb(shdlc, 0); 385 if (skb == NULL) 386 return -ENOMEM; 387 388 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA); 389 } 390 391 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc, 392 struct sk_buff *skb, 393 enum uframe_modifier u_frame_modifier) 394 { 395 u8 w = SHDLC_MAX_WINDOW; 396 bool srej_support = SHDLC_SREJ_SUPPORT; 397 int r; 398 399 pr_debug("u_frame_modifier=%d\n", u_frame_modifier); 400 401 switch (u_frame_modifier) { 402 case U_FRAME_RSET: 403 switch (shdlc->state) { 404 case SHDLC_NEGOTIATING: 405 case SHDLC_CONNECTING: 406 /* 407 * We sent RSET, but chip wants to negotiate or we 408 * got RSET before we managed to send out our. 409 */ 410 if (skb->len > 0) 411 w = skb->data[0]; 412 413 if (skb->len > 1) 414 srej_support = skb->data[1] & 0x01 ? true : 415 false; 416 417 if ((w <= SHDLC_MAX_WINDOW) && 418 (SHDLC_SREJ_SUPPORT || (srej_support == false))) { 419 shdlc->w = w; 420 shdlc->srej_support = srej_support; 421 r = llc_shdlc_connect_send_ua(shdlc); 422 llc_shdlc_connect_complete(shdlc, r); 423 } 424 break; 425 case SHDLC_HALF_CONNECTED: 426 /* 427 * Chip resent RSET due to its timeout - Ignote it 428 * as we already sent UA. 429 */ 430 break; 431 case SHDLC_CONNECTED: 432 /* 433 * Chip wants to reset link. This is unexpected and 434 * unsupported. 435 */ 436 shdlc->hard_fault = -ECONNRESET; 437 break; 438 default: 439 break; 440 } 441 break; 442 case U_FRAME_UA: 443 if ((shdlc->state == SHDLC_CONNECTING && 444 shdlc->connect_tries > 0) || 445 (shdlc->state == SHDLC_NEGOTIATING)) { 446 llc_shdlc_connect_complete(shdlc, 0); 447 shdlc->state = SHDLC_CONNECTED; 448 } 449 break; 450 default: 451 break; 452 } 453 454 kfree_skb(skb); 455 } 456 457 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc) 458 { 459 struct sk_buff *skb; 460 u8 control; 461 int nr; 462 int ns; 463 enum sframe_type s_frame_type; 464 enum uframe_modifier u_frame_modifier; 465 466 if (shdlc->rcv_q.qlen) 467 pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen); 468 469 while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) { 470 control = skb->data[0]; 471 skb_pull(skb, 1); 472 switch (control & SHDLC_CONTROL_HEAD_MASK) { 473 case SHDLC_CONTROL_HEAD_I: 474 case SHDLC_CONTROL_HEAD_I2: 475 if (shdlc->state == SHDLC_HALF_CONNECTED) 476 shdlc->state = SHDLC_CONNECTED; 477 478 ns = (control & SHDLC_CONTROL_NS_MASK) >> 3; 479 nr = control & SHDLC_CONTROL_NR_MASK; 480 llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr); 481 break; 482 case SHDLC_CONTROL_HEAD_S: 483 if (shdlc->state == SHDLC_HALF_CONNECTED) 484 shdlc->state = SHDLC_CONNECTED; 485 486 s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3; 487 nr = control & SHDLC_CONTROL_NR_MASK; 488 llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr); 489 kfree_skb(skb); 490 break; 491 case SHDLC_CONTROL_HEAD_U: 492 u_frame_modifier = control & SHDLC_CONTROL_M_MASK; 493 llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier); 494 break; 495 default: 496 pr_err("UNKNOWN Control=%d\n", control); 497 kfree_skb(skb); 498 break; 499 } 500 } 501 } 502 503 static int llc_shdlc_w_used(int ns, int dnr) 504 { 505 int unack_count; 506 507 if (dnr <= ns) 508 unack_count = ns - dnr; 509 else 510 unack_count = 8 - dnr + ns; 511 512 return unack_count; 513 } 514 515 /* Send frames according to algorithm at spec:10.8.1 */ 516 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc) 517 { 518 struct sk_buff *skb; 519 int r; 520 unsigned long time_sent; 521 522 if (shdlc->send_q.qlen) 523 pr_debug("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n", 524 shdlc->send_q.qlen, shdlc->ns, shdlc->dnr, 525 shdlc->rnr == false ? "false" : "true", 526 shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr), 527 shdlc->ack_pending_q.qlen); 528 529 while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w && 530 (shdlc->rnr == false)) { 531 532 if (shdlc->t1_active) { 533 del_timer_sync(&shdlc->t1_timer); 534 shdlc->t1_active = false; 535 pr_debug("Stopped T1(send ack)\n"); 536 } 537 538 skb = skb_dequeue(&shdlc->send_q); 539 540 *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) | 541 shdlc->nr; 542 543 pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns, 544 shdlc->nr); 545 SHDLC_DUMP_SKB("shdlc frame written", skb); 546 547 r = shdlc->xmit_to_drv(shdlc->hdev, skb); 548 if (r < 0) { 549 shdlc->hard_fault = r; 550 break; 551 } 552 553 shdlc->ns = (shdlc->ns + 1) % 8; 554 555 time_sent = jiffies; 556 *(unsigned long *)skb->cb = time_sent; 557 558 skb_queue_tail(&shdlc->ack_pending_q, skb); 559 560 if (shdlc->t2_active == false) { 561 shdlc->t2_active = true; 562 mod_timer(&shdlc->t2_timer, time_sent + 563 msecs_to_jiffies(SHDLC_T2_VALUE_MS)); 564 pr_debug("Started T2 (retransmit)\n"); 565 } 566 } 567 } 568 569 static void llc_shdlc_connect_timeout(struct timer_list *t) 570 { 571 struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer); 572 573 pr_debug("\n"); 574 575 schedule_work(&shdlc->sm_work); 576 } 577 578 static void llc_shdlc_t1_timeout(struct timer_list *t) 579 { 580 struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer); 581 582 pr_debug("SoftIRQ: need to send ack\n"); 583 584 schedule_work(&shdlc->sm_work); 585 } 586 587 static void llc_shdlc_t2_timeout(struct timer_list *t) 588 { 589 struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer); 590 591 pr_debug("SoftIRQ: need to retransmit\n"); 592 593 schedule_work(&shdlc->sm_work); 594 } 595 596 static void llc_shdlc_sm_work(struct work_struct *work) 597 { 598 struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work); 599 int r; 600 601 pr_debug("\n"); 602 603 mutex_lock(&shdlc->state_mutex); 604 605 switch (shdlc->state) { 606 case SHDLC_DISCONNECTED: 607 skb_queue_purge(&shdlc->rcv_q); 608 skb_queue_purge(&shdlc->send_q); 609 skb_queue_purge(&shdlc->ack_pending_q); 610 break; 611 case SHDLC_CONNECTING: 612 if (shdlc->hard_fault) { 613 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault); 614 break; 615 } 616 617 if (shdlc->connect_tries++ < 5) 618 r = llc_shdlc_connect_initiate(shdlc); 619 else 620 r = -ETIME; 621 if (r < 0) { 622 llc_shdlc_connect_complete(shdlc, r); 623 } else { 624 mod_timer(&shdlc->connect_timer, jiffies + 625 msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS)); 626 627 shdlc->state = SHDLC_NEGOTIATING; 628 } 629 break; 630 case SHDLC_NEGOTIATING: 631 if (timer_pending(&shdlc->connect_timer) == 0) { 632 shdlc->state = SHDLC_CONNECTING; 633 schedule_work(&shdlc->sm_work); 634 } 635 636 llc_shdlc_handle_rcv_queue(shdlc); 637 638 if (shdlc->hard_fault) { 639 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault); 640 break; 641 } 642 break; 643 case SHDLC_HALF_CONNECTED: 644 case SHDLC_CONNECTED: 645 llc_shdlc_handle_rcv_queue(shdlc); 646 llc_shdlc_handle_send_queue(shdlc); 647 648 if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) { 649 pr_debug("Handle T1(send ack) elapsed (T1 now inactive)\n"); 650 651 shdlc->t1_active = false; 652 r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR, 653 shdlc->nr); 654 if (r < 0) 655 shdlc->hard_fault = r; 656 } 657 658 if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) { 659 pr_debug("Handle T2(retransmit) elapsed (T2 inactive)\n"); 660 661 shdlc->t2_active = false; 662 663 llc_shdlc_requeue_ack_pending(shdlc); 664 llc_shdlc_handle_send_queue(shdlc); 665 } 666 667 if (shdlc->hard_fault) 668 shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault); 669 break; 670 default: 671 break; 672 } 673 mutex_unlock(&shdlc->state_mutex); 674 } 675 676 /* 677 * Called from syscall context to establish shdlc link. Sleeps until 678 * link is ready or failure. 679 */ 680 static int llc_shdlc_connect(struct llc_shdlc *shdlc) 681 { 682 DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq); 683 684 pr_debug("\n"); 685 686 mutex_lock(&shdlc->state_mutex); 687 688 shdlc->state = SHDLC_CONNECTING; 689 shdlc->connect_wq = &connect_wq; 690 shdlc->connect_tries = 0; 691 shdlc->connect_result = 1; 692 693 mutex_unlock(&shdlc->state_mutex); 694 695 schedule_work(&shdlc->sm_work); 696 697 wait_event(connect_wq, shdlc->connect_result != 1); 698 699 return shdlc->connect_result; 700 } 701 702 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc) 703 { 704 pr_debug("\n"); 705 706 mutex_lock(&shdlc->state_mutex); 707 708 shdlc->state = SHDLC_DISCONNECTED; 709 710 mutex_unlock(&shdlc->state_mutex); 711 712 schedule_work(&shdlc->sm_work); 713 } 714 715 /* 716 * Receive an incoming shdlc frame. Frame has already been crc-validated. 717 * skb contains only LLC header and payload. 718 * If skb == NULL, it is a notification that the link below is dead. 719 */ 720 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb) 721 { 722 if (skb == NULL) { 723 pr_err("NULL Frame -> link is dead\n"); 724 shdlc->hard_fault = -EREMOTEIO; 725 } else { 726 SHDLC_DUMP_SKB("incoming frame", skb); 727 skb_queue_tail(&shdlc->rcv_q, skb); 728 } 729 730 schedule_work(&shdlc->sm_work); 731 } 732 733 static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv, 734 rcv_to_hci_t rcv_to_hci, int tx_headroom, 735 int tx_tailroom, int *rx_headroom, int *rx_tailroom, 736 llc_failure_t llc_failure) 737 { 738 struct llc_shdlc *shdlc; 739 740 *rx_headroom = SHDLC_LLC_HEAD_ROOM; 741 *rx_tailroom = 0; 742 743 shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL); 744 if (shdlc == NULL) 745 return NULL; 746 747 mutex_init(&shdlc->state_mutex); 748 shdlc->state = SHDLC_DISCONNECTED; 749 750 timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0); 751 timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0); 752 timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0); 753 754 shdlc->w = SHDLC_MAX_WINDOW; 755 shdlc->srej_support = SHDLC_SREJ_SUPPORT; 756 757 skb_queue_head_init(&shdlc->rcv_q); 758 skb_queue_head_init(&shdlc->send_q); 759 skb_queue_head_init(&shdlc->ack_pending_q); 760 761 INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work); 762 763 shdlc->hdev = hdev; 764 shdlc->xmit_to_drv = xmit_to_drv; 765 shdlc->rcv_to_hci = rcv_to_hci; 766 shdlc->tx_headroom = tx_headroom; 767 shdlc->tx_tailroom = tx_tailroom; 768 shdlc->llc_failure = llc_failure; 769 770 return shdlc; 771 } 772 773 static void llc_shdlc_deinit(struct nfc_llc *llc) 774 { 775 struct llc_shdlc *shdlc = nfc_llc_get_data(llc); 776 777 skb_queue_purge(&shdlc->rcv_q); 778 skb_queue_purge(&shdlc->send_q); 779 skb_queue_purge(&shdlc->ack_pending_q); 780 781 kfree(shdlc); 782 } 783 784 static int llc_shdlc_start(struct nfc_llc *llc) 785 { 786 struct llc_shdlc *shdlc = nfc_llc_get_data(llc); 787 788 return llc_shdlc_connect(shdlc); 789 } 790 791 static int llc_shdlc_stop(struct nfc_llc *llc) 792 { 793 struct llc_shdlc *shdlc = nfc_llc_get_data(llc); 794 795 llc_shdlc_disconnect(shdlc); 796 797 return 0; 798 } 799 800 static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb) 801 { 802 struct llc_shdlc *shdlc = nfc_llc_get_data(llc); 803 804 llc_shdlc_recv_frame(shdlc, skb); 805 } 806 807 static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb) 808 { 809 struct llc_shdlc *shdlc = nfc_llc_get_data(llc); 810 811 skb_queue_tail(&shdlc->send_q, skb); 812 813 schedule_work(&shdlc->sm_work); 814 815 return 0; 816 } 817 818 static const struct nfc_llc_ops llc_shdlc_ops = { 819 .init = llc_shdlc_init, 820 .deinit = llc_shdlc_deinit, 821 .start = llc_shdlc_start, 822 .stop = llc_shdlc_stop, 823 .rcv_from_drv = llc_shdlc_rcv_from_drv, 824 .xmit_from_hci = llc_shdlc_xmit_from_hci, 825 }; 826 827 int nfc_llc_shdlc_register(void) 828 { 829 return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops); 830 } 831