xref: /openbmc/linux/net/nfc/hci/llc_shdlc.c (revision 6aa7de05)
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 	*(u8 *)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 	*(u8 *)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_u8(skb, SHDLC_MAX_WINDOW);
386 	skb_put_u8(skb, 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 		*(u8 *)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