1 // SPDX-License-Identifier: GPL-2.0-only
3 * shdlc Link Layer Control
5 * Copyright (C) 2012 Intel Corporation. All rights reserved.
8 #define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
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>
19 SHDLC_DISCONNECTED = 0,
21 SHDLC_NEGOTIATING = 2,
22 SHDLC_HALF_CONNECTED = 3,
27 struct nfc_hci_dev *hdev;
28 xmit_to_drv_t xmit_to_drv;
29 rcv_to_hci_t rcv_to_hci;
31 struct mutex state_mutex;
32 enum shdlc_state state;
35 wait_queue_head_t *connect_wq;
38 struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
40 u8 w; /* window size */
43 struct timer_list t1_timer; /* send ack timeout */
46 struct timer_list t2_timer; /* guard/retransmit timeout */
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 */
53 struct sk_buff_head rcv_q;
55 struct sk_buff_head send_q;
56 bool rnr; /* other side is not ready to receive */
58 struct sk_buff_head ack_pending_q;
60 struct work_struct sm_work;
65 llc_failure_t llc_failure;
68 #define SHDLC_LLC_HEAD_ROOM 2
70 #define SHDLC_MAX_WINDOW 4
71 #define SHDLC_SREJ_SUPPORT false
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
79 #define SHDLC_CONTROL_NS_MASK 0x38
80 #define SHDLC_CONTROL_NR_MASK 0x07
81 #define SHDLC_CONTROL_TYPE_MASK 0x18
83 #define SHDLC_CONTROL_M_MASK 0x1f
92 enum uframe_modifier {
97 #define SHDLC_CONNECT_VALUE_MS 5
98 #define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4)
99 #define SHDLC_T2_VALUE_MS 300
101 #define SHDLC_DUMP_SKB(info, skb) \
103 pr_debug("%s:\n", info); \
104 print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
105 16, 1, skb->data, skb->len, 0); \
108 /* checks x < y <= z modulo 8 */
109 static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
112 return ((x < y) && (y <= z)) ? true : false;
114 return ((y > x) || (y <= z)) ? true : false;
117 /* checks x <= y < z modulo 8 */
118 static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
121 return ((x <= y) && (y < z)) ? true : false;
122 else /* x > z -> z+8 > x */
123 return ((y >= x) || (y < z)) ? true : false;
126 static struct sk_buff *llc_shdlc_alloc_skb(const struct llc_shdlc *shdlc,
131 skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
132 shdlc->tx_tailroom + payload_len, GFP_KERNEL);
134 skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
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)
146 pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
148 skb = llc_shdlc_alloc_skb(shdlc, 0);
152 *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
154 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
161 /* immediately sends an U frame. skb may contain optional payload */
162 static int llc_shdlc_send_u_frame(const struct llc_shdlc *shdlc,
164 enum uframe_modifier uframe_modifier)
168 pr_debug("uframe_modifier=%d\n", uframe_modifier);
170 *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
172 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
180 * Free ack_pending frames until y_nr - 1, and reset t2 according to
181 * the remaining oldest ack_pending frame sent time
183 static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
186 int dnr = shdlc->dnr; /* MUST initially be < y_nr */
188 pr_debug("release ack pending up to frame %d excluded\n", y_nr);
190 while (dnr != y_nr) {
191 pr_debug("release ack pending frame %d\n", dnr);
193 skb = skb_dequeue(&shdlc->ack_pending_q);
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;
204 pr_debug("All sent frames acked. Stopped T2(retransmit)\n");
207 skb = skb_peek(&shdlc->ack_pending_q);
209 mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
210 msecs_to_jiffies(SHDLC_T2_VALUE_MS));
211 shdlc->t2_active = true;
213 pr_debug("Start T2(retransmit) for remaining unacked sent frames\n");
218 * Receive validated frames from lower layer. skb contains HCI payload only.
219 * Handle according to algorithm at spec:10.8.2
221 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
222 struct sk_buff *skb, int ns, int nr)
227 pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
229 if (shdlc->state != SHDLC_CONNECTED)
232 if (x_ns != shdlc->nr) {
233 llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
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");
245 shdlc->rcv_to_hci(shdlc->hdev, skb);
249 shdlc->nr = (shdlc->nr + 1) % 8;
251 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
252 llc_shdlc_reset_t2(shdlc, y_nr);
261 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
263 pr_debug("remote acked up to frame %d excluded\n", y_nr);
265 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
266 llc_shdlc_reset_t2(shdlc, y_nr);
271 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
275 pr_debug("ns reset to %d\n", shdlc->dnr);
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);
281 shdlc->ns = shdlc->dnr;
284 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
288 pr_debug("remote asks retransmission from frame %d\n", y_nr);
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");
297 if (shdlc->dnr != y_nr) {
298 while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
299 skb = skb_dequeue(&shdlc->ack_pending_q);
304 llc_shdlc_requeue_ack_pending(shdlc);
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)
314 if (shdlc->state != SHDLC_CONNECTED)
317 switch (s_frame_type) {
319 llc_shdlc_rcv_ack(shdlc, nr);
320 if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */
322 if (shdlc->send_q.qlen == 0) {
323 skb = llc_shdlc_alloc_skb(shdlc, 0);
325 skb_queue_tail(&shdlc->send_q, skb);
330 llc_shdlc_rcv_rej(shdlc, nr);
333 llc_shdlc_rcv_ack(shdlc, nr);
341 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
343 pr_debug("result=%d\n", r);
345 del_timer_sync(&shdlc->connect_timer);
352 shdlc->state = SHDLC_HALF_CONNECTED;
354 shdlc->state = SHDLC_DISCONNECTED;
357 shdlc->connect_result = r;
359 wake_up(shdlc->connect_wq);
362 static int llc_shdlc_connect_initiate(const struct llc_shdlc *shdlc)
366 skb = llc_shdlc_alloc_skb(shdlc, 2);
370 skb_put_u8(skb, SHDLC_MAX_WINDOW);
371 skb_put_u8(skb, SHDLC_SREJ_SUPPORT ? 1 : 0);
373 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
376 static int llc_shdlc_connect_send_ua(const struct llc_shdlc *shdlc)
380 skb = llc_shdlc_alloc_skb(shdlc, 0);
384 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
387 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
389 enum uframe_modifier u_frame_modifier)
391 u8 w = SHDLC_MAX_WINDOW;
392 bool srej_support = SHDLC_SREJ_SUPPORT;
395 pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
397 switch (u_frame_modifier) {
399 switch (shdlc->state) {
400 case SHDLC_NEGOTIATING:
401 case SHDLC_CONNECTING:
403 * We sent RSET, but chip wants to negotiate or we
404 * got RSET before we managed to send out our.
410 srej_support = skb->data[1] & 0x01 ? true :
413 if ((w <= SHDLC_MAX_WINDOW) &&
414 (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
416 shdlc->srej_support = srej_support;
417 r = llc_shdlc_connect_send_ua(shdlc);
418 llc_shdlc_connect_complete(shdlc, r);
421 case SHDLC_HALF_CONNECTED:
423 * Chip resent RSET due to its timeout - Ignote it
424 * as we already sent UA.
427 case SHDLC_CONNECTED:
429 * Chip wants to reset link. This is unexpected and
432 shdlc->hard_fault = -ECONNRESET;
439 if ((shdlc->state == SHDLC_CONNECTING &&
440 shdlc->connect_tries > 0) ||
441 (shdlc->state == SHDLC_NEGOTIATING)) {
442 llc_shdlc_connect_complete(shdlc, 0);
443 shdlc->state = SHDLC_CONNECTED;
453 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
459 enum sframe_type s_frame_type;
460 enum uframe_modifier u_frame_modifier;
462 if (shdlc->rcv_q.qlen)
463 pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
465 while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
466 control = skb->data[0];
468 switch (control & SHDLC_CONTROL_HEAD_MASK) {
469 case SHDLC_CONTROL_HEAD_I:
470 case SHDLC_CONTROL_HEAD_I2:
471 if (shdlc->state == SHDLC_HALF_CONNECTED)
472 shdlc->state = SHDLC_CONNECTED;
474 ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
475 nr = control & SHDLC_CONTROL_NR_MASK;
476 llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
478 case SHDLC_CONTROL_HEAD_S:
479 if (shdlc->state == SHDLC_HALF_CONNECTED)
480 shdlc->state = SHDLC_CONNECTED;
482 s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
483 nr = control & SHDLC_CONTROL_NR_MASK;
484 llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
487 case SHDLC_CONTROL_HEAD_U:
488 u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
489 llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
492 pr_err("UNKNOWN Control=%d\n", control);
499 static int llc_shdlc_w_used(int ns, int dnr)
504 unack_count = ns - dnr;
506 unack_count = 8 - dnr + ns;
511 /* Send frames according to algorithm at spec:10.8.1 */
512 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
516 unsigned long time_sent;
518 if (shdlc->send_q.qlen)
519 pr_debug("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
520 shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
521 shdlc->rnr == false ? "false" : "true",
522 shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
523 shdlc->ack_pending_q.qlen);
525 while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
526 (shdlc->rnr == false)) {
528 if (shdlc->t1_active) {
529 del_timer_sync(&shdlc->t1_timer);
530 shdlc->t1_active = false;
531 pr_debug("Stopped T1(send ack)\n");
534 skb = skb_dequeue(&shdlc->send_q);
536 *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
539 pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
541 SHDLC_DUMP_SKB("shdlc frame written", skb);
543 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
545 shdlc->hard_fault = r;
549 shdlc->ns = (shdlc->ns + 1) % 8;
552 *(unsigned long *)skb->cb = time_sent;
554 skb_queue_tail(&shdlc->ack_pending_q, skb);
556 if (shdlc->t2_active == false) {
557 shdlc->t2_active = true;
558 mod_timer(&shdlc->t2_timer, time_sent +
559 msecs_to_jiffies(SHDLC_T2_VALUE_MS));
560 pr_debug("Started T2 (retransmit)\n");
565 static void llc_shdlc_connect_timeout(struct timer_list *t)
567 struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);
569 schedule_work(&shdlc->sm_work);
572 static void llc_shdlc_t1_timeout(struct timer_list *t)
574 struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);
576 pr_debug("SoftIRQ: need to send ack\n");
578 schedule_work(&shdlc->sm_work);
581 static void llc_shdlc_t2_timeout(struct timer_list *t)
583 struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);
585 pr_debug("SoftIRQ: need to retransmit\n");
587 schedule_work(&shdlc->sm_work);
590 static void llc_shdlc_sm_work(struct work_struct *work)
592 struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
595 mutex_lock(&shdlc->state_mutex);
597 switch (shdlc->state) {
598 case SHDLC_DISCONNECTED:
599 skb_queue_purge(&shdlc->rcv_q);
600 skb_queue_purge(&shdlc->send_q);
601 skb_queue_purge(&shdlc->ack_pending_q);
603 case SHDLC_CONNECTING:
604 if (shdlc->hard_fault) {
605 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
609 if (shdlc->connect_tries++ < 5)
610 r = llc_shdlc_connect_initiate(shdlc);
614 llc_shdlc_connect_complete(shdlc, r);
616 mod_timer(&shdlc->connect_timer, jiffies +
617 msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
619 shdlc->state = SHDLC_NEGOTIATING;
622 case SHDLC_NEGOTIATING:
623 if (timer_pending(&shdlc->connect_timer) == 0) {
624 shdlc->state = SHDLC_CONNECTING;
625 schedule_work(&shdlc->sm_work);
628 llc_shdlc_handle_rcv_queue(shdlc);
630 if (shdlc->hard_fault) {
631 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
635 case SHDLC_HALF_CONNECTED:
636 case SHDLC_CONNECTED:
637 llc_shdlc_handle_rcv_queue(shdlc);
638 llc_shdlc_handle_send_queue(shdlc);
640 if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
641 pr_debug("Handle T1(send ack) elapsed (T1 now inactive)\n");
643 shdlc->t1_active = false;
644 r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
647 shdlc->hard_fault = r;
650 if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
651 pr_debug("Handle T2(retransmit) elapsed (T2 inactive)\n");
653 shdlc->t2_active = false;
655 llc_shdlc_requeue_ack_pending(shdlc);
656 llc_shdlc_handle_send_queue(shdlc);
659 if (shdlc->hard_fault)
660 shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
665 mutex_unlock(&shdlc->state_mutex);
669 * Called from syscall context to establish shdlc link. Sleeps until
670 * link is ready or failure.
672 static int llc_shdlc_connect(struct llc_shdlc *shdlc)
674 DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
676 mutex_lock(&shdlc->state_mutex);
678 shdlc->state = SHDLC_CONNECTING;
679 shdlc->connect_wq = &connect_wq;
680 shdlc->connect_tries = 0;
681 shdlc->connect_result = 1;
683 mutex_unlock(&shdlc->state_mutex);
685 schedule_work(&shdlc->sm_work);
687 wait_event(connect_wq, shdlc->connect_result != 1);
689 return shdlc->connect_result;
692 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
694 mutex_lock(&shdlc->state_mutex);
696 shdlc->state = SHDLC_DISCONNECTED;
698 mutex_unlock(&shdlc->state_mutex);
700 schedule_work(&shdlc->sm_work);
704 * Receive an incoming shdlc frame. Frame has already been crc-validated.
705 * skb contains only LLC header and payload.
706 * If skb == NULL, it is a notification that the link below is dead.
708 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
711 pr_err("NULL Frame -> link is dead\n");
712 shdlc->hard_fault = -EREMOTEIO;
714 SHDLC_DUMP_SKB("incoming frame", skb);
715 skb_queue_tail(&shdlc->rcv_q, skb);
718 schedule_work(&shdlc->sm_work);
721 static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
722 rcv_to_hci_t rcv_to_hci, int tx_headroom,
723 int tx_tailroom, int *rx_headroom, int *rx_tailroom,
724 llc_failure_t llc_failure)
726 struct llc_shdlc *shdlc;
728 *rx_headroom = SHDLC_LLC_HEAD_ROOM;
731 shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
735 mutex_init(&shdlc->state_mutex);
736 shdlc->state = SHDLC_DISCONNECTED;
738 timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0);
739 timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0);
740 timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0);
742 shdlc->w = SHDLC_MAX_WINDOW;
743 shdlc->srej_support = SHDLC_SREJ_SUPPORT;
745 skb_queue_head_init(&shdlc->rcv_q);
746 skb_queue_head_init(&shdlc->send_q);
747 skb_queue_head_init(&shdlc->ack_pending_q);
749 INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
752 shdlc->xmit_to_drv = xmit_to_drv;
753 shdlc->rcv_to_hci = rcv_to_hci;
754 shdlc->tx_headroom = tx_headroom;
755 shdlc->tx_tailroom = tx_tailroom;
756 shdlc->llc_failure = llc_failure;
761 static void llc_shdlc_deinit(struct nfc_llc *llc)
763 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
765 skb_queue_purge(&shdlc->rcv_q);
766 skb_queue_purge(&shdlc->send_q);
767 skb_queue_purge(&shdlc->ack_pending_q);
772 static int llc_shdlc_start(struct nfc_llc *llc)
774 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
776 return llc_shdlc_connect(shdlc);
779 static int llc_shdlc_stop(struct nfc_llc *llc)
781 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
783 llc_shdlc_disconnect(shdlc);
788 static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
790 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
792 llc_shdlc_recv_frame(shdlc, skb);
795 static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
797 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
799 skb_queue_tail(&shdlc->send_q, skb);
801 schedule_work(&shdlc->sm_work);
806 static const struct nfc_llc_ops llc_shdlc_ops = {
807 .init = llc_shdlc_init,
808 .deinit = llc_shdlc_deinit,
809 .start = llc_shdlc_start,
810 .stop = llc_shdlc_stop,
811 .rcv_from_drv = llc_shdlc_rcv_from_drv,
812 .xmit_from_hci = llc_shdlc_xmit_from_hci,
815 int nfc_llc_shdlc_register(void)
817 return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);