?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Linux 5.7.7
[linux/fpc-iii.git] / net / nfc / hci / llc_shdlc.c
blob0eb4ddc056e789219bf09665a591246eac1b88bf
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 */
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 */
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
126 static struct sk_buff *llc_shdlc_alloc_skb(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. */
140 static int llc_shdlc_send_s_frame(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 */
162 static int llc_shdlc_send_u_frame(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 */
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
205 ("All sent frames acked. Stopped T2(retransmit)\n");
206 }
207 } else {
208 skb = skb_peek(&shdlc->ack_pending_q);
209
210 mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
211 msecs_to_jiffies(SHDLC_T2_VALUE_MS));
212 shdlc->t2_active = true;
213
214 pr_debug
215 ("Start T2(retransmit) for remaining unacked sent frames\n");
216 }
217 }
218
219 /*
220 * Receive validated frames from lower layer. skb contains HCI payload only.
221 * Handle according to algorithm at spec:10.8.2
222 */
223 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
224 struct sk_buff *skb, int ns, int nr)
225 {
226 int x_ns = ns;
227 int y_nr = nr;
228
229 pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
230
231 if (shdlc->state != SHDLC_CONNECTED)
232 goto exit;
233
234 if (x_ns != shdlc->nr) {
235 llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
236 goto exit;
237 }
238
239 if (shdlc->t1_active == false) {
240 shdlc->t1_active = true;
241 mod_timer(&shdlc->t1_timer, jiffies +
242 msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
243 pr_debug("(re)Start T1(send ack)\n");
244 }
245
246 if (skb->len) {
247 shdlc->rcv_to_hci(shdlc->hdev, skb);
248 skb = NULL;
249 }
250
251 shdlc->nr = (shdlc->nr + 1) % 8;
252
253 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
254 llc_shdlc_reset_t2(shdlc, y_nr);
255
256 shdlc->dnr = y_nr;
257 }
258
259 exit:
260 kfree_skb(skb);
261 }
262
263 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
264 {
265 pr_debug("remote acked up to frame %d excluded\n", y_nr);
266
267 if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
268 llc_shdlc_reset_t2(shdlc, y_nr);
269 shdlc->dnr = y_nr;
270 }
271 }
272
273 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
274 {
275 struct sk_buff *skb;
276
277 pr_debug("ns reset to %d\n", shdlc->dnr);
278
279 while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
280 skb_pull(skb, 1); /* remove control field */
281 skb_queue_head(&shdlc->send_q, skb);
282 }
283 shdlc->ns = shdlc->dnr;
284 }
285
286 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
287 {
288 struct sk_buff *skb;
289
290 pr_debug("remote asks retransmission from frame %d\n", y_nr);
291
292 if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
293 if (shdlc->t2_active) {
294 del_timer_sync(&shdlc->t2_timer);
295 shdlc->t2_active = false;
296 pr_debug("Stopped T2(retransmit)\n");
297 }
298
299 if (shdlc->dnr != y_nr) {
300 while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
301 skb = skb_dequeue(&shdlc->ack_pending_q);
302 kfree_skb(skb);
303 }
304 }
305
306 llc_shdlc_requeue_ack_pending(shdlc);
307 }
308 }
309
310 /* See spec RR:10.8.3 REJ:10.8.4 */
311 static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,
312 enum sframe_type s_frame_type, int nr)
313 {
314 struct sk_buff *skb;
315
316 if (shdlc->state != SHDLC_CONNECTED)
317 return;
318
319 switch (s_frame_type) {
320 case S_FRAME_RR:
321 llc_shdlc_rcv_ack(shdlc, nr);
322 if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */
323 shdlc->rnr = false;
324 if (shdlc->send_q.qlen == 0) {
325 skb = llc_shdlc_alloc_skb(shdlc, 0);
326 if (skb)
327 skb_queue_tail(&shdlc->send_q, skb);
328 }
329 }
330 break;
331 case S_FRAME_REJ:
332 llc_shdlc_rcv_rej(shdlc, nr);
333 break;
334 case S_FRAME_RNR:
335 llc_shdlc_rcv_ack(shdlc, nr);
336 shdlc->rnr = true;
337 break;
338 default:
339 break;
340 }
341 }
342
343 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
344 {
345 pr_debug("result=%d\n", r);
346
347 del_timer_sync(&shdlc->connect_timer);
348
349 if (r == 0) {
350 shdlc->ns = 0;
351 shdlc->nr = 0;
352 shdlc->dnr = 0;
353
354 shdlc->state = SHDLC_HALF_CONNECTED;
355 } else {
356 shdlc->state = SHDLC_DISCONNECTED;
357 }
358
359 shdlc->connect_result = r;
360
361 wake_up(shdlc->connect_wq);
362 }
363
364 static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)
365 {
366 struct sk_buff *skb;
367
368 pr_debug("\n");
369
370 skb = llc_shdlc_alloc_skb(shdlc, 2);
371 if (skb == NULL)
372 return -ENOMEM;
373
374 skb_put_u8(skb, SHDLC_MAX_WINDOW);
375 skb_put_u8(skb, SHDLC_SREJ_SUPPORT ? 1 : 0);
376
377 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
378 }
379
380 static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)
381 {
382 struct sk_buff *skb;
383
384 pr_debug("\n");
385
386 skb = llc_shdlc_alloc_skb(shdlc, 0);
387 if (skb == NULL)
388 return -ENOMEM;
389
390 return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
391 }
392
393 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
394 struct sk_buff *skb,
395 enum uframe_modifier u_frame_modifier)
396 {
397 u8 w = SHDLC_MAX_WINDOW;
398 bool srej_support = SHDLC_SREJ_SUPPORT;
399 int r;
400
401 pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
402
403 switch (u_frame_modifier) {
404 case U_FRAME_RSET:
405 switch (shdlc->state) {
406 case SHDLC_NEGOTIATING:
407 case SHDLC_CONNECTING:
408 /*
409 * We sent RSET, but chip wants to negociate or we
410 * got RSET before we managed to send out our.
411 */
412 if (skb->len > 0)
413 w = skb->data[0];
414
415 if (skb->len > 1)
416 srej_support = skb->data[1] & 0x01 ? true :
417 false;
418
419 if ((w <= SHDLC_MAX_WINDOW) &&
420 (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
421 shdlc->w = w;
422 shdlc->srej_support = srej_support;
423 r = llc_shdlc_connect_send_ua(shdlc);
424 llc_shdlc_connect_complete(shdlc, r);
425 }
426 break;
427 case SHDLC_HALF_CONNECTED:
428 /*
429 * Chip resent RSET due to its timeout - Ignote it
430 * as we already sent UA.
431 */
432 break;
433 case SHDLC_CONNECTED:
434 /*
435 * Chip wants to reset link. This is unexpected and
436 * unsupported.
437 */
438 shdlc->hard_fault = -ECONNRESET;
439 break;
440 default:
441 break;
442 }
443 break;
444 case U_FRAME_UA:
445 if ((shdlc->state == SHDLC_CONNECTING &&
446 shdlc->connect_tries > 0) ||
447 (shdlc->state == SHDLC_NEGOTIATING)) {
448 llc_shdlc_connect_complete(shdlc, 0);
449 shdlc->state = SHDLC_CONNECTED;
450 }
451 break;
452 default:
453 break;
454 }
455
456 kfree_skb(skb);
457 }
458
459 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
460 {
461 struct sk_buff *skb;
462 u8 control;
463 int nr;
464 int ns;
465 enum sframe_type s_frame_type;
466 enum uframe_modifier u_frame_modifier;
467
468 if (shdlc->rcv_q.qlen)
469 pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
470
471 while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
472 control = skb->data[0];
473 skb_pull(skb, 1);
474 switch (control & SHDLC_CONTROL_HEAD_MASK) {
475 case SHDLC_CONTROL_HEAD_I:
476 case SHDLC_CONTROL_HEAD_I2:
477 if (shdlc->state == SHDLC_HALF_CONNECTED)
478 shdlc->state = SHDLC_CONNECTED;
479
480 ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
481 nr = control & SHDLC_CONTROL_NR_MASK;
482 llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
483 break;
484 case SHDLC_CONTROL_HEAD_S:
485 if (shdlc->state == SHDLC_HALF_CONNECTED)
486 shdlc->state = SHDLC_CONNECTED;
487
488 s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
489 nr = control & SHDLC_CONTROL_NR_MASK;
490 llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
491 kfree_skb(skb);
492 break;
493 case SHDLC_CONTROL_HEAD_U:
494 u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
495 llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
496 break;
497 default:
498 pr_err("UNKNOWN Control=%d\n", control);
499 kfree_skb(skb);
500 break;
501 }
502 }
503 }
504
505 static int llc_shdlc_w_used(int ns, int dnr)
506 {
507 int unack_count;
508
509 if (dnr <= ns)
510 unack_count = ns - dnr;
511 else
512 unack_count = 8 - dnr + ns;
513
514 return unack_count;
515 }
516
517 /* Send frames according to algorithm at spec:10.8.1 */
518 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
519 {
520 struct sk_buff *skb;
521 int r;
522 unsigned long time_sent;
523
524 if (shdlc->send_q.qlen)
525 pr_debug
526 ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
527 shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
528 shdlc->rnr == false ? "false" : "true",
529 shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
530 shdlc->ack_pending_q.qlen);
531
532 while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
533 (shdlc->rnr == false)) {
534
535 if (shdlc->t1_active) {
536 del_timer_sync(&shdlc->t1_timer);
537 shdlc->t1_active = false;
538 pr_debug("Stopped T1(send ack)\n");
539 }
540
541 skb = skb_dequeue(&shdlc->send_q);
542
543 *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
544 shdlc->nr;
545
546 pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
547 shdlc->nr);
548 SHDLC_DUMP_SKB("shdlc frame written", skb);
549
550 r = shdlc->xmit_to_drv(shdlc->hdev, skb);
551 if (r < 0) {
552 shdlc->hard_fault = r;
553 break;
554 }
555
556 shdlc->ns = (shdlc->ns + 1) % 8;
557
558 time_sent = jiffies;
559 *(unsigned long *)skb->cb = time_sent;
560
561 skb_queue_tail(&shdlc->ack_pending_q, skb);
562
563 if (shdlc->t2_active == false) {
564 shdlc->t2_active = true;
565 mod_timer(&shdlc->t2_timer, time_sent +
566 msecs_to_jiffies(SHDLC_T2_VALUE_MS));
567 pr_debug("Started T2 (retransmit)\n");
568 }
569 }
570 }
571
572 static void llc_shdlc_connect_timeout(struct timer_list *t)
573 {
574 struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);
575
576 pr_debug("\n");
577
578 schedule_work(&shdlc->sm_work);
579 }
580
581 static void llc_shdlc_t1_timeout(struct timer_list *t)
582 {
583 struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);
584
585 pr_debug("SoftIRQ: need to send ack\n");
586
587 schedule_work(&shdlc->sm_work);
588 }
589
590 static void llc_shdlc_t2_timeout(struct timer_list *t)
591 {
592 struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);
593
594 pr_debug("SoftIRQ: need to retransmit\n");
595
596 schedule_work(&shdlc->sm_work);
597 }
598
599 static void llc_shdlc_sm_work(struct work_struct *work)
600 {
601 struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
602 int r;
603
604 pr_debug("\n");
605
606 mutex_lock(&shdlc->state_mutex);
607
608 switch (shdlc->state) {
609 case SHDLC_DISCONNECTED:
610 skb_queue_purge(&shdlc->rcv_q);
611 skb_queue_purge(&shdlc->send_q);
612 skb_queue_purge(&shdlc->ack_pending_q);
613 break;
614 case SHDLC_CONNECTING:
615 if (shdlc->hard_fault) {
616 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
617 break;
618 }
619
620 if (shdlc->connect_tries++ < 5)
621 r = llc_shdlc_connect_initiate(shdlc);
622 else
623 r = -ETIME;
624 if (r < 0) {
625 llc_shdlc_connect_complete(shdlc, r);
626 } else {
627 mod_timer(&shdlc->connect_timer, jiffies +
628 msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
629
630 shdlc->state = SHDLC_NEGOTIATING;
631 }
632 break;
633 case SHDLC_NEGOTIATING:
634 if (timer_pending(&shdlc->connect_timer) == 0) {
635 shdlc->state = SHDLC_CONNECTING;
636 schedule_work(&shdlc->sm_work);
637 }
638
639 llc_shdlc_handle_rcv_queue(shdlc);
640
641 if (shdlc->hard_fault) {
642 llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
643 break;
644 }
645 break;
646 case SHDLC_HALF_CONNECTED:
647 case SHDLC_CONNECTED:
648 llc_shdlc_handle_rcv_queue(shdlc);
649 llc_shdlc_handle_send_queue(shdlc);
650
651 if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
652 pr_debug
653 ("Handle T1(send ack) elapsed (T1 now inactive)\n");
654
655 shdlc->t1_active = false;
656 r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
657 shdlc->nr);
658 if (r < 0)
659 shdlc->hard_fault = r;
660 }
661
662 if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
663 pr_debug
664 ("Handle T2(retransmit) elapsed (T2 inactive)\n");
665
666 shdlc->t2_active = false;
667
668 llc_shdlc_requeue_ack_pending(shdlc);
669 llc_shdlc_handle_send_queue(shdlc);
670 }
671
672 if (shdlc->hard_fault)
673 shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
674 break;
675 default:
676 break;
677 }
678 mutex_unlock(&shdlc->state_mutex);
679 }
680
681 /*
682 * Called from syscall context to establish shdlc link. Sleeps until
683 * link is ready or failure.
684 */
685 static int llc_shdlc_connect(struct llc_shdlc *shdlc)
686 {
687 DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
688
689 pr_debug("\n");
690
691 mutex_lock(&shdlc->state_mutex);
692
693 shdlc->state = SHDLC_CONNECTING;
694 shdlc->connect_wq = &connect_wq;
695 shdlc->connect_tries = 0;
696 shdlc->connect_result = 1;
697
698 mutex_unlock(&shdlc->state_mutex);
699
700 schedule_work(&shdlc->sm_work);
701
702 wait_event(connect_wq, shdlc->connect_result != 1);
703
704 return shdlc->connect_result;
705 }
706
707 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
708 {
709 pr_debug("\n");
710
711 mutex_lock(&shdlc->state_mutex);
712
713 shdlc->state = SHDLC_DISCONNECTED;
714
715 mutex_unlock(&shdlc->state_mutex);
716
717 schedule_work(&shdlc->sm_work);
718 }
719
720 /*
721 * Receive an incoming shdlc frame. Frame has already been crc-validated.
722 * skb contains only LLC header and payload.
723 * If skb == NULL, it is a notification that the link below is dead.
724 */
725 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
726 {
727 if (skb == NULL) {
728 pr_err("NULL Frame -> link is dead\n");
729 shdlc->hard_fault = -EREMOTEIO;
730 } else {
731 SHDLC_DUMP_SKB("incoming frame", skb);
732 skb_queue_tail(&shdlc->rcv_q, skb);
733 }
734
735 schedule_work(&shdlc->sm_work);
736 }
737
738 static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
739 rcv_to_hci_t rcv_to_hci, int tx_headroom,
740 int tx_tailroom, int *rx_headroom, int *rx_tailroom,
741 llc_failure_t llc_failure)
742 {
743 struct llc_shdlc *shdlc;
744
745 *rx_headroom = SHDLC_LLC_HEAD_ROOM;
746 *rx_tailroom = 0;
747
748 shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
749 if (shdlc == NULL)
750 return NULL;
751
752 mutex_init(&shdlc->state_mutex);
753 shdlc->state = SHDLC_DISCONNECTED;
754
755 timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0);
756 timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0);
757 timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0);
758
759 shdlc->w = SHDLC_MAX_WINDOW;
760 shdlc->srej_support = SHDLC_SREJ_SUPPORT;
761
762 skb_queue_head_init(&shdlc->rcv_q);
763 skb_queue_head_init(&shdlc->send_q);
764 skb_queue_head_init(&shdlc->ack_pending_q);
765
766 INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
767
768 shdlc->hdev = hdev;
769 shdlc->xmit_to_drv = xmit_to_drv;
770 shdlc->rcv_to_hci = rcv_to_hci;
771 shdlc->tx_headroom = tx_headroom;
772 shdlc->tx_tailroom = tx_tailroom;
773 shdlc->llc_failure = llc_failure;
774
775 return shdlc;
776 }
777
778 static void llc_shdlc_deinit(struct nfc_llc *llc)
779 {
780 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
781
782 skb_queue_purge(&shdlc->rcv_q);
783 skb_queue_purge(&shdlc->send_q);
784 skb_queue_purge(&shdlc->ack_pending_q);
785
786 kfree(shdlc);
787 }
788
789 static int llc_shdlc_start(struct nfc_llc *llc)
790 {
791 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
792
793 return llc_shdlc_connect(shdlc);
794 }
795
796 static int llc_shdlc_stop(struct nfc_llc *llc)
797 {
798 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
799
800 llc_shdlc_disconnect(shdlc);
801
802 return 0;
803 }
804
805 static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
806 {
807 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
808
809 llc_shdlc_recv_frame(shdlc, skb);
810 }
811
812 static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
813 {
814 struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
815
816 skb_queue_tail(&shdlc->send_q, skb);
817
818 schedule_work(&shdlc->sm_work);
819
820 return 0;
821 }
822
823 static struct nfc_llc_ops llc_shdlc_ops = {
824 .init = llc_shdlc_init,
825 .deinit = llc_shdlc_deinit,
826 .start = llc_shdlc_start,
827 .stop = llc_shdlc_stop,
828 .rcv_from_drv = llc_shdlc_rcv_from_drv,
829 .xmit_from_hci = llc_shdlc_xmit_from_hci,
830 };
831
832 int nfc_llc_shdlc_register(void)
833 {
834 return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
835 }
Linux with added FPC-III support