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