2 * shdlc Link Layer Control
4 * Copyright (C) 2012 Intel Corporation. All rights reserved.
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.
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.
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.
21 #define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
23 #include <linux/types.h>
24 #include <linux/sched.h>
25 #include <linux/export.h>
26 #include <linux/wait.h>
27 #include <linux/slab.h>
28 #include <linux/skbuff.h>
33 SHDLC_DISCONNECTED = 0,
35 SHDLC_NEGOCIATING = 2,
40 struct nfc_hci_dev *hdev;
41 xmit_to_drv_t xmit_to_drv;
42 rcv_to_hci_t rcv_to_hci;
44 struct mutex state_mutex;
45 enum shdlc_state state;
48 wait_queue_head_t *connect_wq;
51 struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
53 u8 w; /* window size */
56 struct timer_list t1_timer; /* send ack timeout */
59 struct timer_list t2_timer; /* guard/retransmit timeout */
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 */
66 struct sk_buff_head rcv_q;
68 struct sk_buff_head send_q;
69 bool rnr; /* other side is not ready to receive */
71 struct sk_buff_head ack_pending_q;
73 struct work_struct sm_work;
78 llc_failure_t llc_failure;
81 #define SHDLC_LLC_HEAD_ROOM 2
83 #define SHDLC_MAX_WINDOW 4
84 #define SHDLC_SREJ_SUPPORT false
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
92 #define SHDLC_CONTROL_NS_MASK 0x38
93 #define SHDLC_CONTROL_NR_MASK 0x07
94 #define SHDLC_CONTROL_TYPE_MASK 0x18
96 #define SHDLC_CONTROL_M_MASK 0x1f
105 enum uframe_modifier {
110 #define SHDLC_CONNECT_VALUE_MS 5
111 #define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4)
112 #define SHDLC_T2_VALUE_MS 300
114 #define SHDLC_DUMP_SKB(info, skb) \
116 pr_debug("%s:\n", info); \
117 print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
118 16, 1, skb->data, skb->len, 0); \
121 /* checks x < y <= z modulo 8 */
122 static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
125 return ((x < y) && (y <= z)) ? true : false;
127 return ((y > x) || (y <= z)) ? true : false;
130 /* checks x <= y < z modulo 8 */
131 static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
134 return ((x <= y) && (y < z)) ? true : false;
135 else /* x > z -> z+8 > x */
136 return ((y >= x) || (y < z)) ? true : false;
139 static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc,
144 skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
145 shdlc->tx_tailroom + payload_len, GFP_KERNEL);
147 skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
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)
159 pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
161 skb = llc_shdlc_alloc_skb(shdlc, 0);
165 *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
167 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
174 /* immediately sends an U frame. skb may contain optional payload */
175 static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc,
177 enum uframe_modifier uframe_modifier)
181 pr_debug("uframe_modifier=%d\n", uframe_modifier);
183 *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
185 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
193 * Free ack_pending frames until y_nr - 1, and reset t2 according to
194 * the remaining oldest ack_pending frame sent time
196 static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
199 int dnr = shdlc->dnr; /* MUST initially be < y_nr */
201 pr_debug("release ack pending up to frame %d excluded\n", y_nr);
203 while (dnr != y_nr) {
204 pr_debug("release ack pending frame %d\n", dnr);
206 skb = skb_dequeue(&shdlc->ack_pending_q);
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;
218 ("All sent frames acked. Stopped T2(retransmit)\n");
221 skb = skb_peek(&shdlc->ack_pending_q);
223 mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
224 msecs_to_jiffies(SHDLC_T2_VALUE_MS));
225 shdlc->t2_active = true;
228 ("Start T2(retransmit) for remaining unacked sent frames\n");
233 * Receive validated frames from lower layer. skb contains HCI payload only.
234 * Handle according to algorithm at spec:10.8.2
236 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
237 struct sk_buff *skb, int ns, int nr)
242 pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
244 if (shdlc->state != SHDLC_CONNECTED)
247 if (x_ns != shdlc->nr) {
248 llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
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");
260 shdlc->rcv_to_hci(shdlc->hdev, skb);
264 shdlc->nr = (shdlc->nr + 1) % 8;
266 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
267 llc_shdlc_reset_t2(shdlc, y_nr);
276 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
278 pr_debug("remote acked up to frame %d excluded\n", y_nr);
280 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
281 llc_shdlc_reset_t2(shdlc, y_nr);
286 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
290 pr_debug("ns reset to %d\n", shdlc->dnr);
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);
296 shdlc->ns = shdlc->dnr;
299 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
303 pr_debug("remote asks retransmition from frame %d\n", y_nr);
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");
312 if (shdlc->dnr != y_nr) {
313 while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
314 skb = skb_dequeue(&shdlc->ack_pending_q);
319 llc_shdlc_requeue_ack_pending(shdlc);
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)
329 if (shdlc->state != SHDLC_CONNECTED)
332 switch (s_frame_type) {
334 llc_shdlc_rcv_ack(shdlc, nr);
335 if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */
337 if (shdlc->send_q.qlen == 0) {
338 skb = llc_shdlc_alloc_skb(shdlc, 0);
340 skb_queue_tail(&shdlc->send_q, skb);
345 llc_shdlc_rcv_rej(shdlc, nr);
348 llc_shdlc_rcv_ack(shdlc, nr);
356 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
358 pr_debug("result=%d\n", r);
360 del_timer_sync(&shdlc->connect_timer);
367 shdlc->state = SHDLC_CONNECTED;
369 shdlc->state = SHDLC_DISCONNECTED;
372 shdlc->connect_result = r;
374 wake_up(shdlc->connect_wq);
377 static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)
383 skb = llc_shdlc_alloc_skb(shdlc, 2);
387 *skb_put(skb, 1) = SHDLC_MAX_WINDOW;
388 *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0;
390 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
393 static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)
399 skb = llc_shdlc_alloc_skb(shdlc, 0);
403 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
406 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
408 enum uframe_modifier u_frame_modifier)
410 u8 w = SHDLC_MAX_WINDOW;
411 bool srej_support = SHDLC_SREJ_SUPPORT;
414 pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
416 switch (u_frame_modifier) {
418 if (shdlc->state == SHDLC_NEGOCIATING) {
419 /* we sent RSET, but chip wants to negociate */
424 srej_support = skb->data[1] & 0x01 ? true :
427 if ((w <= SHDLC_MAX_WINDOW) &&
428 (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
430 shdlc->srej_support = srej_support;
431 r = llc_shdlc_connect_send_ua(shdlc);
432 llc_shdlc_connect_complete(shdlc, r);
434 } else if (shdlc->state == SHDLC_CONNECTED) {
436 * Chip wants to reset link. This is unexpected and
439 shdlc->hard_fault = -ECONNRESET;
443 if ((shdlc->state == SHDLC_CONNECTING &&
444 shdlc->connect_tries > 0) ||
445 (shdlc->state == SHDLC_NEGOCIATING))
446 llc_shdlc_connect_complete(shdlc, 0);
455 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
461 enum sframe_type s_frame_type;
462 enum uframe_modifier u_frame_modifier;
464 if (shdlc->rcv_q.qlen)
465 pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
467 while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
468 control = skb->data[0];
470 switch (control & SHDLC_CONTROL_HEAD_MASK) {
471 case SHDLC_CONTROL_HEAD_I:
472 case SHDLC_CONTROL_HEAD_I2:
473 ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
474 nr = control & SHDLC_CONTROL_NR_MASK;
475 llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
477 case SHDLC_CONTROL_HEAD_S:
478 s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
479 nr = control & SHDLC_CONTROL_NR_MASK;
480 llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
483 case SHDLC_CONTROL_HEAD_U:
484 u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
485 llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
488 pr_err("UNKNOWN Control=%d\n", control);
495 static int llc_shdlc_w_used(int ns, int dnr)
500 unack_count = ns - dnr;
502 unack_count = 8 - dnr + ns;
507 /* Send frames according to algorithm at spec:10.8.1 */
508 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
512 unsigned long time_sent;
514 if (shdlc->send_q.qlen)
516 ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
517 shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
518 shdlc->rnr == false ? "false" : "true",
519 shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
520 shdlc->ack_pending_q.qlen);
522 while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
523 (shdlc->rnr == false)) {
525 if (shdlc->t1_active) {
526 del_timer_sync(&shdlc->t1_timer);
527 shdlc->t1_active = false;
528 pr_debug("Stopped T1(send ack)\n");
531 skb = skb_dequeue(&shdlc->send_q);
533 *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
536 pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
538 SHDLC_DUMP_SKB("shdlc frame written", skb);
540 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
542 shdlc->hard_fault = r;
546 shdlc->ns = (shdlc->ns + 1) % 8;
549 *(unsigned long *)skb->cb = time_sent;
551 skb_queue_tail(&shdlc->ack_pending_q, skb);
553 if (shdlc->t2_active == false) {
554 shdlc->t2_active = true;
555 mod_timer(&shdlc->t2_timer, time_sent +
556 msecs_to_jiffies(SHDLC_T2_VALUE_MS));
557 pr_debug("Started T2 (retransmit)\n");
562 static void llc_shdlc_connect_timeout(unsigned long data)
564 struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
568 queue_work(system_nrt_wq, &shdlc->sm_work);
571 static void llc_shdlc_t1_timeout(unsigned long data)
573 struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
575 pr_debug("SoftIRQ: need to send ack\n");
577 queue_work(system_nrt_wq, &shdlc->sm_work);
580 static void llc_shdlc_t2_timeout(unsigned long data)
582 struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
584 pr_debug("SoftIRQ: need to retransmit\n");
586 queue_work(system_nrt_wq, &shdlc->sm_work);
589 static void llc_shdlc_sm_work(struct work_struct *work)
591 struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
596 mutex_lock(&shdlc->state_mutex);
598 switch (shdlc->state) {
599 case SHDLC_DISCONNECTED:
600 skb_queue_purge(&shdlc->rcv_q);
601 skb_queue_purge(&shdlc->send_q);
602 skb_queue_purge(&shdlc->ack_pending_q);
604 case SHDLC_CONNECTING:
605 if (shdlc->hard_fault) {
606 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
610 if (shdlc->connect_tries++ < 5)
611 r = llc_shdlc_connect_initiate(shdlc);
615 llc_shdlc_connect_complete(shdlc, r);
617 mod_timer(&shdlc->connect_timer, jiffies +
618 msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
620 shdlc->state = SHDLC_NEGOCIATING;
623 case SHDLC_NEGOCIATING:
624 if (timer_pending(&shdlc->connect_timer) == 0) {
625 shdlc->state = SHDLC_CONNECTING;
626 queue_work(system_nrt_wq, &shdlc->sm_work);
629 llc_shdlc_handle_rcv_queue(shdlc);
631 if (shdlc->hard_fault) {
632 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
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) {
642 ("Handle T1(send ack) elapsed (T1 now inactive)\n");
644 shdlc->t1_active = false;
645 r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
648 shdlc->hard_fault = r;
651 if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
653 ("Handle T2(retransmit) elapsed (T2 inactive)\n");
655 shdlc->t2_active = false;
657 llc_shdlc_requeue_ack_pending(shdlc);
658 llc_shdlc_handle_send_queue(shdlc);
661 if (shdlc->hard_fault) {
662 shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
668 mutex_unlock(&shdlc->state_mutex);
672 * Called from syscall context to establish shdlc link. Sleeps until
673 * link is ready or failure.
675 static int llc_shdlc_connect(struct llc_shdlc *shdlc)
677 DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
681 mutex_lock(&shdlc->state_mutex);
683 shdlc->state = SHDLC_CONNECTING;
684 shdlc->connect_wq = &connect_wq;
685 shdlc->connect_tries = 0;
686 shdlc->connect_result = 1;
688 mutex_unlock(&shdlc->state_mutex);
690 queue_work(system_nrt_wq, &shdlc->sm_work);
692 wait_event(connect_wq, shdlc->connect_result != 1);
694 return shdlc->connect_result;
697 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
701 mutex_lock(&shdlc->state_mutex);
703 shdlc->state = SHDLC_DISCONNECTED;
705 mutex_unlock(&shdlc->state_mutex);
707 queue_work(system_nrt_wq, &shdlc->sm_work);
711 * Receive an incoming shdlc frame. Frame has already been crc-validated.
712 * skb contains only LLC header and payload.
713 * If skb == NULL, it is a notification that the link below is dead.
715 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
718 pr_err("NULL Frame -> link is dead\n");
719 shdlc->hard_fault = -EREMOTEIO;
721 SHDLC_DUMP_SKB("incoming frame", skb);
722 skb_queue_tail(&shdlc->rcv_q, skb);
725 queue_work(system_nrt_wq, &shdlc->sm_work);
728 static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
729 rcv_to_hci_t rcv_to_hci, int tx_headroom,
730 int tx_tailroom, int *rx_headroom, int *rx_tailroom,
731 llc_failure_t llc_failure)
733 struct llc_shdlc *shdlc;
735 *rx_headroom = SHDLC_LLC_HEAD_ROOM;
738 shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
742 mutex_init(&shdlc->state_mutex);
743 shdlc->state = SHDLC_DISCONNECTED;
745 init_timer(&shdlc->connect_timer);
746 shdlc->connect_timer.data = (unsigned long)shdlc;
747 shdlc->connect_timer.function = llc_shdlc_connect_timeout;
749 init_timer(&shdlc->t1_timer);
750 shdlc->t1_timer.data = (unsigned long)shdlc;
751 shdlc->t1_timer.function = llc_shdlc_t1_timeout;
753 init_timer(&shdlc->t2_timer);
754 shdlc->t2_timer.data = (unsigned long)shdlc;
755 shdlc->t2_timer.function = llc_shdlc_t2_timeout;
757 shdlc->w = SHDLC_MAX_WINDOW;
758 shdlc->srej_support = SHDLC_SREJ_SUPPORT;
760 skb_queue_head_init(&shdlc->rcv_q);
761 skb_queue_head_init(&shdlc->send_q);
762 skb_queue_head_init(&shdlc->ack_pending_q);
764 INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
767 shdlc->xmit_to_drv = xmit_to_drv;
768 shdlc->rcv_to_hci = rcv_to_hci;
769 shdlc->tx_headroom = tx_headroom;
770 shdlc->tx_tailroom = tx_tailroom;
771 shdlc->llc_failure = llc_failure;
776 static void llc_shdlc_deinit(struct nfc_llc *llc)
778 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
780 skb_queue_purge(&shdlc->rcv_q);
781 skb_queue_purge(&shdlc->send_q);
782 skb_queue_purge(&shdlc->ack_pending_q);
787 static int llc_shdlc_start(struct nfc_llc *llc)
789 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
791 return llc_shdlc_connect(shdlc);
794 static int llc_shdlc_stop(struct nfc_llc *llc)
796 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
798 llc_shdlc_disconnect(shdlc);
803 static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
805 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
807 llc_shdlc_recv_frame(shdlc, skb);
810 static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
812 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
814 skb_queue_tail(&shdlc->send_q, skb);
816 queue_work(system_nrt_wq, &shdlc->sm_work);
821 static struct nfc_llc_ops llc_shdlc_ops = {
822 .init = llc_shdlc_init,
823 .deinit = llc_shdlc_deinit,
824 .start = llc_shdlc_start,
825 .stop = llc_shdlc_stop,
826 .rcv_from_drv = llc_shdlc_rcv_from_drv,
827 .xmit_from_hci = llc_shdlc_xmit_from_hci,
830 int nfc_llc_shdlc_register()
832 return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
834 EXPORT_SYMBOL(nfc_llc_shdlc_register);