xref: /openbmc/linux/net/nfc/hci/llc_shdlc.c (revision 5b25a5bf)
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 */
llc_shdlc_x_lt_y_lteq_z(int x,int y,int z)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 */
llc_shdlc_x_lteq_y_lt_z(int x,int y,int z)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 
llc_shdlc_alloc_skb(const struct llc_shdlc * shdlc,int payload_len)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. */
llc_shdlc_send_s_frame(const struct llc_shdlc * shdlc,enum sframe_type sframe_type,int nr)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 */
llc_shdlc_send_u_frame(const struct llc_shdlc * shdlc,struct sk_buff * skb,enum uframe_modifier uframe_modifier)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  */
llc_shdlc_reset_t2(struct llc_shdlc * shdlc,int y_nr)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  */
llc_shdlc_rcv_i_frame(struct llc_shdlc * shdlc,struct sk_buff * skb,int ns,int nr)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 
llc_shdlc_rcv_ack(struct llc_shdlc * shdlc,int y_nr)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 
llc_shdlc_requeue_ack_pending(struct llc_shdlc * shdlc)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 
llc_shdlc_rcv_rej(struct llc_shdlc * shdlc,int y_nr)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 */
llc_shdlc_rcv_s_frame(struct llc_shdlc * shdlc,enum sframe_type s_frame_type,int nr)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 
llc_shdlc_connect_complete(struct llc_shdlc * shdlc,int r)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 
llc_shdlc_connect_initiate(const struct llc_shdlc * shdlc)362 static int llc_shdlc_connect_initiate(const struct llc_shdlc *shdlc)
363 {
364 	struct sk_buff *skb;
365 
366 	skb = llc_shdlc_alloc_skb(shdlc, 2);
367 	if (skb == NULL)
368 		return -ENOMEM;
369 
370 	skb_put_u8(skb, SHDLC_MAX_WINDOW);
371 	skb_put_u8(skb, SHDLC_SREJ_SUPPORT ? 1 : 0);
372 
373 	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
374 }
375 
llc_shdlc_connect_send_ua(const struct llc_shdlc * shdlc)376 static int llc_shdlc_connect_send_ua(const struct llc_shdlc *shdlc)
377 {
378 	struct sk_buff *skb;
379 
380 	skb = llc_shdlc_alloc_skb(shdlc, 0);
381 	if (skb == NULL)
382 		return -ENOMEM;
383 
384 	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
385 }
386 
llc_shdlc_rcv_u_frame(struct llc_shdlc * shdlc,struct sk_buff * skb,enum uframe_modifier u_frame_modifier)387 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
388 				  struct sk_buff *skb,
389 				  enum uframe_modifier u_frame_modifier)
390 {
391 	u8 w = SHDLC_MAX_WINDOW;
392 	bool srej_support = SHDLC_SREJ_SUPPORT;
393 	int r;
394 
395 	pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
396 
397 	switch (u_frame_modifier) {
398 	case U_FRAME_RSET:
399 		switch (shdlc->state) {
400 		case SHDLC_NEGOTIATING:
401 		case SHDLC_CONNECTING:
402 			/*
403 			 * We sent RSET, but chip wants to negotiate or we
404 			 * got RSET before we managed to send out our.
405 			 */
406 			if (skb->len > 0)
407 				w = skb->data[0];
408 
409 			if (skb->len > 1)
410 				srej_support = skb->data[1] & 0x01 ? true :
411 					       false;
412 
413 			if ((w <= SHDLC_MAX_WINDOW) &&
414 			    (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
415 				shdlc->w = w;
416 				shdlc->srej_support = srej_support;
417 				r = llc_shdlc_connect_send_ua(shdlc);
418 				llc_shdlc_connect_complete(shdlc, r);
419 			}
420 			break;
421 		case SHDLC_HALF_CONNECTED:
422 			/*
423 			 * Chip resent RSET due to its timeout - Ignote it
424 			 * as we already sent UA.
425 			 */
426 			break;
427 		case SHDLC_CONNECTED:
428 			/*
429 			 * Chip wants to reset link. This is unexpected and
430 			 * unsupported.
431 			 */
432 			shdlc->hard_fault = -ECONNRESET;
433 			break;
434 		default:
435 			break;
436 		}
437 		break;
438 	case U_FRAME_UA:
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;
444 		}
445 		break;
446 	default:
447 		break;
448 	}
449 
450 	kfree_skb(skb);
451 }
452 
llc_shdlc_handle_rcv_queue(struct llc_shdlc * shdlc)453 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
454 {
455 	struct sk_buff *skb;
456 	u8 control;
457 	int nr;
458 	int ns;
459 	enum sframe_type s_frame_type;
460 	enum uframe_modifier u_frame_modifier;
461 
462 	if (shdlc->rcv_q.qlen)
463 		pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
464 
465 	while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
466 		control = skb->data[0];
467 		skb_pull(skb, 1);
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;
473 
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);
477 			break;
478 		case SHDLC_CONTROL_HEAD_S:
479 			if (shdlc->state == SHDLC_HALF_CONNECTED)
480 				shdlc->state = SHDLC_CONNECTED;
481 
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);
485 			kfree_skb(skb);
486 			break;
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);
490 			break;
491 		default:
492 			pr_err("UNKNOWN Control=%d\n", control);
493 			kfree_skb(skb);
494 			break;
495 		}
496 	}
497 }
498 
llc_shdlc_w_used(int ns,int dnr)499 static int llc_shdlc_w_used(int ns, int dnr)
500 {
501 	int unack_count;
502 
503 	if (dnr <= ns)
504 		unack_count = ns - dnr;
505 	else
506 		unack_count = 8 - dnr + ns;
507 
508 	return unack_count;
509 }
510 
511 /* Send frames according to algorithm at spec:10.8.1 */
llc_shdlc_handle_send_queue(struct llc_shdlc * shdlc)512 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
513 {
514 	struct sk_buff *skb;
515 	int r;
516 	unsigned long time_sent;
517 
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);
524 
525 	while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
526 	       (shdlc->rnr == false)) {
527 
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");
532 		}
533 
534 		skb = skb_dequeue(&shdlc->send_q);
535 
536 		*(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
537 					shdlc->nr;
538 
539 		pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
540 			 shdlc->nr);
541 		SHDLC_DUMP_SKB("shdlc frame written", skb);
542 
543 		r = shdlc->xmit_to_drv(shdlc->hdev, skb);
544 		if (r < 0) {
545 			shdlc->hard_fault = r;
546 			break;
547 		}
548 
549 		shdlc->ns = (shdlc->ns + 1) % 8;
550 
551 		time_sent = jiffies;
552 		*(unsigned long *)skb->cb = time_sent;
553 
554 		skb_queue_tail(&shdlc->ack_pending_q, skb);
555 
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");
561 		}
562 	}
563 }
564 
llc_shdlc_connect_timeout(struct timer_list * t)565 static void llc_shdlc_connect_timeout(struct timer_list *t)
566 {
567 	struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);
568 
569 	schedule_work(&shdlc->sm_work);
570 }
571 
llc_shdlc_t1_timeout(struct timer_list * t)572 static void llc_shdlc_t1_timeout(struct timer_list *t)
573 {
574 	struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);
575 
576 	pr_debug("SoftIRQ: need to send ack\n");
577 
578 	schedule_work(&shdlc->sm_work);
579 }
580 
llc_shdlc_t2_timeout(struct timer_list * t)581 static void llc_shdlc_t2_timeout(struct timer_list *t)
582 {
583 	struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);
584 
585 	pr_debug("SoftIRQ: need to retransmit\n");
586 
587 	schedule_work(&shdlc->sm_work);
588 }
589 
llc_shdlc_sm_work(struct work_struct * work)590 static void llc_shdlc_sm_work(struct work_struct *work)
591 {
592 	struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
593 	int r;
594 
595 	mutex_lock(&shdlc->state_mutex);
596 
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);
602 		break;
603 	case SHDLC_CONNECTING:
604 		if (shdlc->hard_fault) {
605 			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
606 			break;
607 		}
608 
609 		if (shdlc->connect_tries++ < 5)
610 			r = llc_shdlc_connect_initiate(shdlc);
611 		else
612 			r = -ETIME;
613 		if (r < 0) {
614 			llc_shdlc_connect_complete(shdlc, r);
615 		} else {
616 			mod_timer(&shdlc->connect_timer, jiffies +
617 				  msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
618 
619 			shdlc->state = SHDLC_NEGOTIATING;
620 		}
621 		break;
622 	case SHDLC_NEGOTIATING:
623 		if (timer_pending(&shdlc->connect_timer) == 0) {
624 			shdlc->state = SHDLC_CONNECTING;
625 			schedule_work(&shdlc->sm_work);
626 		}
627 
628 		llc_shdlc_handle_rcv_queue(shdlc);
629 
630 		if (shdlc->hard_fault) {
631 			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
632 			break;
633 		}
634 		break;
635 	case SHDLC_HALF_CONNECTED:
636 	case SHDLC_CONNECTED:
637 		llc_shdlc_handle_rcv_queue(shdlc);
638 		llc_shdlc_handle_send_queue(shdlc);
639 
640 		if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
641 			pr_debug("Handle T1(send ack) elapsed (T1 now inactive)\n");
642 
643 			shdlc->t1_active = false;
644 			r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
645 						   shdlc->nr);
646 			if (r < 0)
647 				shdlc->hard_fault = r;
648 		}
649 
650 		if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
651 			pr_debug("Handle T2(retransmit) elapsed (T2 inactive)\n");
652 
653 			shdlc->t2_active = false;
654 
655 			llc_shdlc_requeue_ack_pending(shdlc);
656 			llc_shdlc_handle_send_queue(shdlc);
657 		}
658 
659 		if (shdlc->hard_fault)
660 			shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
661 		break;
662 	default:
663 		break;
664 	}
665 	mutex_unlock(&shdlc->state_mutex);
666 }
667 
668 /*
669  * Called from syscall context to establish shdlc link. Sleeps until
670  * link is ready or failure.
671  */
llc_shdlc_connect(struct llc_shdlc * shdlc)672 static int llc_shdlc_connect(struct llc_shdlc *shdlc)
673 {
674 	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
675 
676 	mutex_lock(&shdlc->state_mutex);
677 
678 	shdlc->state = SHDLC_CONNECTING;
679 	shdlc->connect_wq = &connect_wq;
680 	shdlc->connect_tries = 0;
681 	shdlc->connect_result = 1;
682 
683 	mutex_unlock(&shdlc->state_mutex);
684 
685 	schedule_work(&shdlc->sm_work);
686 
687 	wait_event(connect_wq, shdlc->connect_result != 1);
688 
689 	return shdlc->connect_result;
690 }
691 
llc_shdlc_disconnect(struct llc_shdlc * shdlc)692 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
693 {
694 	mutex_lock(&shdlc->state_mutex);
695 
696 	shdlc->state = SHDLC_DISCONNECTED;
697 
698 	mutex_unlock(&shdlc->state_mutex);
699 
700 	schedule_work(&shdlc->sm_work);
701 }
702 
703 /*
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.
707  */
llc_shdlc_recv_frame(struct llc_shdlc * shdlc,struct sk_buff * skb)708 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
709 {
710 	if (skb == NULL) {
711 		pr_err("NULL Frame -> link is dead\n");
712 		shdlc->hard_fault = -EREMOTEIO;
713 	} else {
714 		SHDLC_DUMP_SKB("incoming frame", skb);
715 		skb_queue_tail(&shdlc->rcv_q, skb);
716 	}
717 
718 	schedule_work(&shdlc->sm_work);
719 }
720 
llc_shdlc_init(struct nfc_hci_dev * hdev,xmit_to_drv_t xmit_to_drv,rcv_to_hci_t rcv_to_hci,int tx_headroom,int tx_tailroom,int * rx_headroom,int * rx_tailroom,llc_failure_t llc_failure)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)
725 {
726 	struct llc_shdlc *shdlc;
727 
728 	*rx_headroom = SHDLC_LLC_HEAD_ROOM;
729 	*rx_tailroom = 0;
730 
731 	shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
732 	if (shdlc == NULL)
733 		return NULL;
734 
735 	mutex_init(&shdlc->state_mutex);
736 	shdlc->state = SHDLC_DISCONNECTED;
737 
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);
741 
742 	shdlc->w = SHDLC_MAX_WINDOW;
743 	shdlc->srej_support = SHDLC_SREJ_SUPPORT;
744 
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);
748 
749 	INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
750 
751 	shdlc->hdev = hdev;
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;
757 
758 	return shdlc;
759 }
760 
llc_shdlc_deinit(struct nfc_llc * llc)761 static void llc_shdlc_deinit(struct nfc_llc *llc)
762 {
763 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
764 
765 	skb_queue_purge(&shdlc->rcv_q);
766 	skb_queue_purge(&shdlc->send_q);
767 	skb_queue_purge(&shdlc->ack_pending_q);
768 
769 	kfree(shdlc);
770 }
771 
llc_shdlc_start(struct nfc_llc * llc)772 static int llc_shdlc_start(struct nfc_llc *llc)
773 {
774 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
775 
776 	return llc_shdlc_connect(shdlc);
777 }
778 
llc_shdlc_stop(struct nfc_llc * llc)779 static int llc_shdlc_stop(struct nfc_llc *llc)
780 {
781 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
782 
783 	llc_shdlc_disconnect(shdlc);
784 
785 	return 0;
786 }
787 
llc_shdlc_rcv_from_drv(struct nfc_llc * llc,struct sk_buff * skb)788 static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
789 {
790 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
791 
792 	llc_shdlc_recv_frame(shdlc, skb);
793 }
794 
llc_shdlc_xmit_from_hci(struct nfc_llc * llc,struct sk_buff * skb)795 static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
796 {
797 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
798 
799 	skb_queue_tail(&shdlc->send_q, skb);
800 
801 	schedule_work(&shdlc->sm_work);
802 
803 	return 0;
804 }
805 
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,
813 };
814 
nfc_llc_shdlc_register(void)815 int nfc_llc_shdlc_register(void)
816 {
817 	return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
818 }
819