networking: add and use skb_put_u8()
[sfrench/cifs-2.6.git] / net / nfc / hci / llc_shdlc.c
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 }