Merge tag 'v4.4-rc1' into patchwork
[linux-block.git] / drivers / isdn / hisax / isdnl2.c
1 /* $Id: isdnl2.c,v 2.30.2.4 2004/02/11 13:21:34 keil Exp $
2  *
3  * Author       Karsten Keil
4  *              based on the teles driver from Jan den Ouden
5  * Copyright    by Karsten Keil      <keil@isdn4linux.de>
6  *
7  * This software may be used and distributed according to the terms
8  * of the GNU General Public License, incorporated herein by reference.
9  *
10  * For changes and modifications please read
11  * Documentation/isdn/HiSax.cert
12  *
13  * Thanks to    Jan den Ouden
14  *              Fritz Elfert
15  *
16  */
17
18 #include <linux/init.h>
19 #include <linux/gfp.h>
20 #include "hisax.h"
21 #include "isdnl2.h"
22
23 const char *l2_revision = "$Revision: 2.30.2.4 $";
24
25 static void l2m_debug(struct FsmInst *fi, char *fmt, ...);
26
27 static struct Fsm l2fsm;
28
29 enum {
30         ST_L2_1,
31         ST_L2_2,
32         ST_L2_3,
33         ST_L2_4,
34         ST_L2_5,
35         ST_L2_6,
36         ST_L2_7,
37         ST_L2_8,
38 };
39
40 #define L2_STATE_COUNT (ST_L2_8 + 1)
41
42 static char *strL2State[] =
43 {
44         "ST_L2_1",
45         "ST_L2_2",
46         "ST_L2_3",
47         "ST_L2_4",
48         "ST_L2_5",
49         "ST_L2_6",
50         "ST_L2_7",
51         "ST_L2_8",
52 };
53
54 enum {
55         EV_L2_UI,
56         EV_L2_SABME,
57         EV_L2_DISC,
58         EV_L2_DM,
59         EV_L2_UA,
60         EV_L2_FRMR,
61         EV_L2_SUPER,
62         EV_L2_I,
63         EV_L2_DL_DATA,
64         EV_L2_ACK_PULL,
65         EV_L2_DL_UNIT_DATA,
66         EV_L2_DL_ESTABLISH_REQ,
67         EV_L2_DL_RELEASE_REQ,
68         EV_L2_MDL_ASSIGN,
69         EV_L2_MDL_REMOVE,
70         EV_L2_MDL_ERROR,
71         EV_L1_DEACTIVATE,
72         EV_L2_T200,
73         EV_L2_T203,
74         EV_L2_SET_OWN_BUSY,
75         EV_L2_CLEAR_OWN_BUSY,
76         EV_L2_FRAME_ERROR,
77 };
78
79 #define L2_EVENT_COUNT (EV_L2_FRAME_ERROR + 1)
80
81 static char *strL2Event[] =
82 {
83         "EV_L2_UI",
84         "EV_L2_SABME",
85         "EV_L2_DISC",
86         "EV_L2_DM",
87         "EV_L2_UA",
88         "EV_L2_FRMR",
89         "EV_L2_SUPER",
90         "EV_L2_I",
91         "EV_L2_DL_DATA",
92         "EV_L2_ACK_PULL",
93         "EV_L2_DL_UNIT_DATA",
94         "EV_L2_DL_ESTABLISH_REQ",
95         "EV_L2_DL_RELEASE_REQ",
96         "EV_L2_MDL_ASSIGN",
97         "EV_L2_MDL_REMOVE",
98         "EV_L2_MDL_ERROR",
99         "EV_L1_DEACTIVATE",
100         "EV_L2_T200",
101         "EV_L2_T203",
102         "EV_L2_SET_OWN_BUSY",
103         "EV_L2_CLEAR_OWN_BUSY",
104         "EV_L2_FRAME_ERROR",
105 };
106
107 static int l2addrsize(struct Layer2 *l2);
108
109 static void
110 set_peer_busy(struct Layer2 *l2) {
111         test_and_set_bit(FLG_PEER_BUSY, &l2->flag);
112         if (!skb_queue_empty(&l2->i_queue) ||
113             !skb_queue_empty(&l2->ui_queue))
114                 test_and_set_bit(FLG_L2BLOCK, &l2->flag);
115 }
116
117 static void
118 clear_peer_busy(struct Layer2 *l2) {
119         if (test_and_clear_bit(FLG_PEER_BUSY, &l2->flag))
120                 test_and_clear_bit(FLG_L2BLOCK, &l2->flag);
121 }
122
123 static void
124 InitWin(struct Layer2 *l2)
125 {
126         int i;
127
128         for (i = 0; i < MAX_WINDOW; i++)
129                 l2->windowar[i] = NULL;
130 }
131
132 static int
133 freewin1(struct Layer2 *l2)
134 {
135         int i, cnt = 0;
136
137         for (i = 0; i < MAX_WINDOW; i++) {
138                 if (l2->windowar[i]) {
139                         cnt++;
140                         dev_kfree_skb(l2->windowar[i]);
141                         l2->windowar[i] = NULL;
142                 }
143         }
144         return cnt;
145 }
146
147 static inline void
148 freewin(struct PStack *st)
149 {
150         freewin1(&st->l2);
151 }
152
153 static void
154 ReleaseWin(struct Layer2 *l2)
155 {
156         int cnt;
157
158         if ((cnt = freewin1(l2)))
159                 printk(KERN_WARNING "isdl2 freed %d skbuffs in release\n", cnt);
160 }
161
162 static inline unsigned int
163 cansend(struct PStack *st)
164 {
165         unsigned int p1;
166
167         if (test_bit(FLG_MOD128, &st->l2.flag))
168                 p1 = (st->l2.vs - st->l2.va) % 128;
169         else
170                 p1 = (st->l2.vs - st->l2.va) % 8;
171         return ((p1 < st->l2.window) && !test_bit(FLG_PEER_BUSY, &st->l2.flag));
172 }
173
174 static inline void
175 clear_exception(struct Layer2 *l2)
176 {
177         test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
178         test_and_clear_bit(FLG_REJEXC, &l2->flag);
179         test_and_clear_bit(FLG_OWN_BUSY, &l2->flag);
180         clear_peer_busy(l2);
181 }
182
183 static inline int
184 l2headersize(struct Layer2 *l2, int ui)
185 {
186         return (((test_bit(FLG_MOD128, &l2->flag) && (!ui)) ? 2 : 1) +
187                 (test_bit(FLG_LAPD, &l2->flag) ? 2 : 1));
188 }
189
190 inline int
191 l2addrsize(struct Layer2 *l2)
192 {
193         return (test_bit(FLG_LAPD, &l2->flag) ? 2 : 1);
194 }
195
196 static int
197 sethdraddr(struct Layer2 *l2, u_char *header, int rsp)
198 {
199         u_char *ptr = header;
200         int crbit = rsp;
201
202         if (test_bit(FLG_LAPD, &l2->flag)) {
203                 *ptr++ = (l2->sap << 2) | (rsp ? 2 : 0);
204                 *ptr++ = (l2->tei << 1) | 1;
205                 return (2);
206         } else {
207                 if (test_bit(FLG_ORIG, &l2->flag))
208                         crbit = !crbit;
209                 if (crbit)
210                         *ptr++ = 1;
211                 else
212                         *ptr++ = 3;
213                 return (1);
214         }
215 }
216
217 static inline void
218 enqueue_super(struct PStack *st,
219               struct sk_buff *skb)
220 {
221         if (test_bit(FLG_LAPB, &st->l2.flag))
222                 st->l1.bcs->tx_cnt += skb->len;
223         st->l2.l2l1(st, PH_DATA | REQUEST, skb);
224 }
225
226 #define enqueue_ui(a, b) enqueue_super(a, b)
227
228 static inline int
229 IsUI(u_char *data)
230 {
231         return ((data[0] & 0xef) == UI);
232 }
233
234 static inline int
235 IsUA(u_char *data)
236 {
237         return ((data[0] & 0xef) == UA);
238 }
239
240 static inline int
241 IsDM(u_char *data)
242 {
243         return ((data[0] & 0xef) == DM);
244 }
245
246 static inline int
247 IsDISC(u_char *data)
248 {
249         return ((data[0] & 0xef) == DISC);
250 }
251
252 static inline int
253 IsSFrame(u_char *data, struct PStack *st)
254 {
255         register u_char d = *data;
256
257         if (!test_bit(FLG_MOD128, &st->l2.flag))
258                 d &= 0xf;
259         return (((d & 0xf3) == 1) && ((d & 0x0c) != 0x0c));
260 }
261
262 static inline int
263 IsSABME(u_char *data, struct PStack *st)
264 {
265         u_char d = data[0] & ~0x10;
266
267         return (test_bit(FLG_MOD128, &st->l2.flag) ? d == SABME : d == SABM);
268 }
269
270 static inline int
271 IsREJ(u_char *data, struct PStack *st)
272 {
273         return (test_bit(FLG_MOD128, &st->l2.flag) ? data[0] == REJ : (data[0] & 0xf) == REJ);
274 }
275
276 static inline int
277 IsFRMR(u_char *data)
278 {
279         return ((data[0] & 0xef) == FRMR);
280 }
281
282 static inline int
283 IsRNR(u_char *data, struct PStack *st)
284 {
285         return (test_bit(FLG_MOD128, &st->l2.flag) ? data[0] == RNR : (data[0] & 0xf) == RNR);
286 }
287
288 static int
289 iframe_error(struct PStack *st, struct sk_buff *skb)
290 {
291         int i = l2addrsize(&st->l2) + (test_bit(FLG_MOD128, &st->l2.flag) ? 2 : 1);
292         int rsp = *skb->data & 0x2;
293
294         if (test_bit(FLG_ORIG, &st->l2.flag))
295                 rsp = !rsp;
296
297         if (rsp)
298                 return 'L';
299
300
301         if (skb->len < i)
302                 return 'N';
303
304         if ((skb->len - i) > st->l2.maxlen)
305                 return 'O';
306
307
308         return 0;
309 }
310
311 static int
312 super_error(struct PStack *st, struct sk_buff *skb)
313 {
314         if (skb->len != l2addrsize(&st->l2) +
315             (test_bit(FLG_MOD128, &st->l2.flag) ? 2 : 1))
316                 return 'N';
317
318         return 0;
319 }
320
321 static int
322 unnum_error(struct PStack *st, struct sk_buff *skb, int wantrsp)
323 {
324         int rsp = (*skb->data & 0x2) >> 1;
325         if (test_bit(FLG_ORIG, &st->l2.flag))
326                 rsp = !rsp;
327
328         if (rsp != wantrsp)
329                 return 'L';
330
331         if (skb->len != l2addrsize(&st->l2) + 1)
332                 return 'N';
333
334         return 0;
335 }
336
337 static int
338 UI_error(struct PStack *st, struct sk_buff *skb)
339 {
340         int rsp = *skb->data & 0x2;
341         if (test_bit(FLG_ORIG, &st->l2.flag))
342                 rsp = !rsp;
343
344         if (rsp)
345                 return 'L';
346
347         if (skb->len > st->l2.maxlen + l2addrsize(&st->l2) + 1)
348                 return 'O';
349
350         return 0;
351 }
352
353 static int
354 FRMR_error(struct PStack *st, struct sk_buff *skb)
355 {
356         int headers = l2addrsize(&st->l2) + 1;
357         u_char *datap = skb->data + headers;
358         int rsp = *skb->data & 0x2;
359
360         if (test_bit(FLG_ORIG, &st->l2.flag))
361                 rsp = !rsp;
362
363         if (!rsp)
364                 return 'L';
365
366         if (test_bit(FLG_MOD128, &st->l2.flag)) {
367                 if (skb->len < headers + 5)
368                         return 'N';
369                 else
370                         l2m_debug(&st->l2.l2m, "FRMR information %2x %2x %2x %2x %2x",
371                                   datap[0], datap[1], datap[2],
372                                   datap[3], datap[4]);
373         } else {
374                 if (skb->len < headers + 3)
375                         return 'N';
376                 else
377                         l2m_debug(&st->l2.l2m, "FRMR information %2x %2x %2x",
378                                   datap[0], datap[1], datap[2]);
379         }
380
381         return 0;
382 }
383
384 static unsigned int
385 legalnr(struct PStack *st, unsigned int nr)
386 {
387         struct Layer2 *l2 = &st->l2;
388
389         if (test_bit(FLG_MOD128, &l2->flag))
390                 return ((nr - l2->va) % 128) <= ((l2->vs - l2->va) % 128);
391         else
392                 return ((nr - l2->va) % 8) <= ((l2->vs - l2->va) % 8);
393 }
394
395 static void
396 setva(struct PStack *st, unsigned int nr)
397 {
398         struct Layer2 *l2 = &st->l2;
399         int len;
400         u_long flags;
401
402         spin_lock_irqsave(&l2->lock, flags);
403         while (l2->va != nr) {
404                 (l2->va)++;
405                 if (test_bit(FLG_MOD128, &l2->flag))
406                         l2->va %= 128;
407                 else
408                         l2->va %= 8;
409                 len = l2->windowar[l2->sow]->len;
410                 if (PACKET_NOACK == l2->windowar[l2->sow]->pkt_type)
411                         len = -1;
412                 dev_kfree_skb(l2->windowar[l2->sow]);
413                 l2->windowar[l2->sow] = NULL;
414                 l2->sow = (l2->sow + 1) % l2->window;
415                 spin_unlock_irqrestore(&l2->lock, flags);
416                 if (test_bit(FLG_LLI_L2WAKEUP, &st->lli.flag) && (len >= 0))
417                         lli_writewakeup(st, len);
418                 spin_lock_irqsave(&l2->lock, flags);
419         }
420         spin_unlock_irqrestore(&l2->lock, flags);
421 }
422
423 static void
424 send_uframe(struct PStack *st, u_char cmd, u_char cr)
425 {
426         struct sk_buff *skb;
427         u_char tmp[MAX_HEADER_LEN];
428         int i;
429
430         i = sethdraddr(&st->l2, tmp, cr);
431         tmp[i++] = cmd;
432         if (!(skb = alloc_skb(i, GFP_ATOMIC))) {
433                 printk(KERN_WARNING "isdl2 can't alloc sbbuff for send_uframe\n");
434                 return;
435         }
436         memcpy(skb_put(skb, i), tmp, i);
437         enqueue_super(st, skb);
438 }
439
440 static inline u_char
441 get_PollFlag(struct PStack *st, struct sk_buff *skb)
442 {
443         return (skb->data[l2addrsize(&(st->l2))] & 0x10);
444 }
445
446 static inline u_char
447 get_PollFlagFree(struct PStack *st, struct sk_buff *skb)
448 {
449         u_char PF;
450
451         PF = get_PollFlag(st, skb);
452         dev_kfree_skb(skb);
453         return (PF);
454 }
455
456 static inline void
457 start_t200(struct PStack *st, int i)
458 {
459         FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, i);
460         test_and_set_bit(FLG_T200_RUN, &st->l2.flag);
461 }
462
463 static inline void
464 restart_t200(struct PStack *st, int i)
465 {
466         FsmRestartTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, i);
467         test_and_set_bit(FLG_T200_RUN, &st->l2.flag);
468 }
469
470 static inline void
471 stop_t200(struct PStack *st, int i)
472 {
473         if (test_and_clear_bit(FLG_T200_RUN, &st->l2.flag))
474                 FsmDelTimer(&st->l2.t200, i);
475 }
476
477 static inline void
478 st5_dl_release_l2l3(struct PStack *st)
479 {
480         int pr;
481
482         if (test_and_clear_bit(FLG_PEND_REL, &st->l2.flag))
483                 pr = DL_RELEASE | CONFIRM;
484         else
485                 pr = DL_RELEASE | INDICATION;
486
487         st->l2.l2l3(st, pr, NULL);
488 }
489
490 static inline void
491 lapb_dl_release_l2l3(struct PStack *st, int f)
492 {
493         if (test_bit(FLG_LAPB, &st->l2.flag))
494                 st->l2.l2l1(st, PH_DEACTIVATE | REQUEST, NULL);
495         st->l2.l2l3(st, DL_RELEASE | f, NULL);
496 }
497
498 static void
499 establishlink(struct FsmInst *fi)
500 {
501         struct PStack *st = fi->userdata;
502         u_char cmd;
503
504         clear_exception(&st->l2);
505         st->l2.rc = 0;
506         cmd = (test_bit(FLG_MOD128, &st->l2.flag) ? SABME : SABM) | 0x10;
507         send_uframe(st, cmd, CMD);
508         FsmDelTimer(&st->l2.t203, 1);
509         restart_t200(st, 1);
510         test_and_clear_bit(FLG_PEND_REL, &st->l2.flag);
511         freewin(st);
512         FsmChangeState(fi, ST_L2_5);
513 }
514
515 static void
516 l2_mdl_error_ua(struct FsmInst *fi, int event, void *arg)
517 {
518         struct sk_buff *skb = arg;
519         struct PStack *st = fi->userdata;
520
521         if (get_PollFlagFree(st, skb))
522                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'C');
523         else
524                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'D');
525 }
526
527 static void
528 l2_mdl_error_dm(struct FsmInst *fi, int event, void *arg)
529 {
530         struct sk_buff *skb = arg;
531         struct PStack *st = fi->userdata;
532
533         if (get_PollFlagFree(st, skb))
534                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'B');
535         else {
536                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'E');
537                 establishlink(fi);
538                 test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
539         }
540 }
541
542 static void
543 l2_st8_mdl_error_dm(struct FsmInst *fi, int event, void *arg)
544 {
545         struct sk_buff *skb = arg;
546         struct PStack *st = fi->userdata;
547
548         if (get_PollFlagFree(st, skb))
549                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'B');
550         else {
551                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'E');
552         }
553         establishlink(fi);
554         test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
555 }
556
557 static void
558 l2_go_st3(struct FsmInst *fi, int event, void *arg)
559 {
560         FsmChangeState(fi, ST_L2_3);
561 }
562
563 static void
564 l2_mdl_assign(struct FsmInst *fi, int event, void *arg)
565 {
566         struct PStack *st = fi->userdata;
567
568         FsmChangeState(fi, ST_L2_3);
569         st->l2.l2tei(st, MDL_ASSIGN | INDICATION, NULL);
570 }
571
572 static void
573 l2_queue_ui_assign(struct FsmInst *fi, int event, void *arg)
574 {
575         struct PStack *st = fi->userdata;
576         struct sk_buff *skb = arg;
577
578         skb_queue_tail(&st->l2.ui_queue, skb);
579         FsmChangeState(fi, ST_L2_2);
580         st->l2.l2tei(st, MDL_ASSIGN | INDICATION, NULL);
581 }
582
583 static void
584 l2_queue_ui(struct FsmInst *fi, int event, void *arg)
585 {
586         struct PStack *st = fi->userdata;
587         struct sk_buff *skb = arg;
588
589         skb_queue_tail(&st->l2.ui_queue, skb);
590 }
591
592 static void
593 tx_ui(struct PStack *st)
594 {
595         struct sk_buff *skb;
596         u_char header[MAX_HEADER_LEN];
597         int i;
598
599         i = sethdraddr(&(st->l2), header, CMD);
600         header[i++] = UI;
601         while ((skb = skb_dequeue(&st->l2.ui_queue))) {
602                 memcpy(skb_push(skb, i), header, i);
603                 enqueue_ui(st, skb);
604         }
605 }
606
607 static void
608 l2_send_ui(struct FsmInst *fi, int event, void *arg)
609 {
610         struct PStack *st = fi->userdata;
611         struct sk_buff *skb = arg;
612
613         skb_queue_tail(&st->l2.ui_queue, skb);
614         tx_ui(st);
615 }
616
617 static void
618 l2_got_ui(struct FsmInst *fi, int event, void *arg)
619 {
620         struct PStack *st = fi->userdata;
621         struct sk_buff *skb = arg;
622
623         skb_pull(skb, l2headersize(&st->l2, 1));
624         st->l2.l2l3(st, DL_UNIT_DATA | INDICATION, skb);
625 /*      ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
626  *              in states 1-3 for broadcast
627  */
628
629
630 }
631
632 static void
633 l2_establish(struct FsmInst *fi, int event, void *arg)
634 {
635         struct PStack *st = fi->userdata;
636
637         establishlink(fi);
638         test_and_set_bit(FLG_L3_INIT, &st->l2.flag);
639 }
640
641 static void
642 l2_discard_i_setl3(struct FsmInst *fi, int event, void *arg)
643 {
644         struct PStack *st = fi->userdata;
645
646         skb_queue_purge(&st->l2.i_queue);
647         test_and_set_bit(FLG_L3_INIT, &st->l2.flag);
648         test_and_clear_bit(FLG_PEND_REL, &st->l2.flag);
649 }
650
651 static void
652 l2_l3_reestablish(struct FsmInst *fi, int event, void *arg)
653 {
654         struct PStack *st = fi->userdata;
655
656         skb_queue_purge(&st->l2.i_queue);
657         establishlink(fi);
658         test_and_set_bit(FLG_L3_INIT, &st->l2.flag);
659 }
660
661 static void
662 l2_release(struct FsmInst *fi, int event, void *arg)
663 {
664         struct PStack *st = fi->userdata;
665
666         st->l2.l2l3(st, DL_RELEASE | CONFIRM, NULL);
667 }
668
669 static void
670 l2_pend_rel(struct FsmInst *fi, int event, void *arg)
671 {
672         struct PStack *st = fi->userdata;
673
674         test_and_set_bit(FLG_PEND_REL, &st->l2.flag);
675 }
676
677 static void
678 l2_disconnect(struct FsmInst *fi, int event, void *arg)
679 {
680         struct PStack *st = fi->userdata;
681
682         skb_queue_purge(&st->l2.i_queue);
683         freewin(st);
684         FsmChangeState(fi, ST_L2_6);
685         st->l2.rc = 0;
686         send_uframe(st, DISC | 0x10, CMD);
687         FsmDelTimer(&st->l2.t203, 1);
688         restart_t200(st, 2);
689 }
690
691 static void
692 l2_start_multi(struct FsmInst *fi, int event, void *arg)
693 {
694         struct PStack *st = fi->userdata;
695         struct sk_buff *skb = arg;
696
697         send_uframe(st, UA | get_PollFlagFree(st, skb), RSP);
698
699         clear_exception(&st->l2);
700         st->l2.vs = 0;
701         st->l2.va = 0;
702         st->l2.vr = 0;
703         st->l2.sow = 0;
704         FsmChangeState(fi, ST_L2_7);
705         FsmAddTimer(&st->l2.t203, st->l2.T203, EV_L2_T203, NULL, 3);
706
707         st->l2.l2l3(st, DL_ESTABLISH | INDICATION, NULL);
708 }
709
710 static void
711 l2_send_UA(struct FsmInst *fi, int event, void *arg)
712 {
713         struct PStack *st = fi->userdata;
714         struct sk_buff *skb = arg;
715
716         send_uframe(st, UA | get_PollFlagFree(st, skb), RSP);
717 }
718
719 static void
720 l2_send_DM(struct FsmInst *fi, int event, void *arg)
721 {
722         struct PStack *st = fi->userdata;
723         struct sk_buff *skb = arg;
724
725         send_uframe(st, DM | get_PollFlagFree(st, skb), RSP);
726 }
727
728 static void
729 l2_restart_multi(struct FsmInst *fi, int event, void *arg)
730 {
731         struct PStack *st = fi->userdata;
732         struct sk_buff *skb = arg;
733         int est = 0, state;
734
735         state = fi->state;
736
737         send_uframe(st, UA | get_PollFlagFree(st, skb), RSP);
738
739         st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'F');
740
741         if (st->l2.vs != st->l2.va) {
742                 skb_queue_purge(&st->l2.i_queue);
743                 est = 1;
744         }
745
746         clear_exception(&st->l2);
747         st->l2.vs = 0;
748         st->l2.va = 0;
749         st->l2.vr = 0;
750         st->l2.sow = 0;
751         FsmChangeState(fi, ST_L2_7);
752         stop_t200(st, 3);
753         FsmRestartTimer(&st->l2.t203, st->l2.T203, EV_L2_T203, NULL, 3);
754
755         if (est)
756                 st->l2.l2l3(st, DL_ESTABLISH | INDICATION, NULL);
757
758         if ((ST_L2_7 == state) || (ST_L2_8 == state))
759                 if (!skb_queue_empty(&st->l2.i_queue) && cansend(st))
760                         st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
761 }
762
763 static void
764 l2_stop_multi(struct FsmInst *fi, int event, void *arg)
765 {
766         struct PStack *st = fi->userdata;
767         struct sk_buff *skb = arg;
768
769         FsmChangeState(fi, ST_L2_4);
770         FsmDelTimer(&st->l2.t203, 3);
771         stop_t200(st, 4);
772
773         send_uframe(st, UA | get_PollFlagFree(st, skb), RSP);
774
775         skb_queue_purge(&st->l2.i_queue);
776         freewin(st);
777         lapb_dl_release_l2l3(st, INDICATION);
778 }
779
780 static void
781 l2_connected(struct FsmInst *fi, int event, void *arg)
782 {
783         struct PStack *st = fi->userdata;
784         struct sk_buff *skb = arg;
785         int pr = -1;
786
787         if (!get_PollFlag(st, skb)) {
788                 l2_mdl_error_ua(fi, event, arg);
789                 return;
790         }
791         dev_kfree_skb(skb);
792
793         if (test_and_clear_bit(FLG_PEND_REL, &st->l2.flag))
794                 l2_disconnect(fi, event, arg);
795
796         if (test_and_clear_bit(FLG_L3_INIT, &st->l2.flag)) {
797                 pr = DL_ESTABLISH | CONFIRM;
798         } else if (st->l2.vs != st->l2.va) {
799                 skb_queue_purge(&st->l2.i_queue);
800                 pr = DL_ESTABLISH | INDICATION;
801         }
802
803         stop_t200(st, 5);
804
805         st->l2.vr = 0;
806         st->l2.vs = 0;
807         st->l2.va = 0;
808         st->l2.sow = 0;
809         FsmChangeState(fi, ST_L2_7);
810         FsmAddTimer(&st->l2.t203, st->l2.T203, EV_L2_T203, NULL, 4);
811
812         if (pr != -1)
813                 st->l2.l2l3(st, pr, NULL);
814
815         if (!skb_queue_empty(&st->l2.i_queue) && cansend(st))
816                 st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
817 }
818
819 static void
820 l2_released(struct FsmInst *fi, int event, void *arg)
821 {
822         struct PStack *st = fi->userdata;
823         struct sk_buff *skb = arg;
824
825         if (!get_PollFlag(st, skb)) {
826                 l2_mdl_error_ua(fi, event, arg);
827                 return;
828         }
829         dev_kfree_skb(skb);
830
831         stop_t200(st, 6);
832         lapb_dl_release_l2l3(st, CONFIRM);
833         FsmChangeState(fi, ST_L2_4);
834 }
835
836 static void
837 l2_reestablish(struct FsmInst *fi, int event, void *arg)
838 {
839         struct PStack *st = fi->userdata;
840         struct sk_buff *skb = arg;
841
842         if (!get_PollFlagFree(st, skb)) {
843                 establishlink(fi);
844                 test_and_set_bit(FLG_L3_INIT, &st->l2.flag);
845         }
846 }
847
848 static void
849 l2_st5_dm_release(struct FsmInst *fi, int event, void *arg)
850 {
851         struct PStack *st = fi->userdata;
852         struct sk_buff *skb = arg;
853
854         if (get_PollFlagFree(st, skb)) {
855                 stop_t200(st, 7);
856                 if (!test_bit(FLG_L3_INIT, &st->l2.flag))
857                         skb_queue_purge(&st->l2.i_queue);
858                 if (test_bit(FLG_LAPB, &st->l2.flag))
859                         st->l2.l2l1(st, PH_DEACTIVATE | REQUEST, NULL);
860                 st5_dl_release_l2l3(st);
861                 FsmChangeState(fi, ST_L2_4);
862         }
863 }
864
865 static void
866 l2_st6_dm_release(struct FsmInst *fi, int event, void *arg)
867 {
868         struct PStack *st = fi->userdata;
869         struct sk_buff *skb = arg;
870
871         if (get_PollFlagFree(st, skb)) {
872                 stop_t200(st, 8);
873                 lapb_dl_release_l2l3(st, CONFIRM);
874                 FsmChangeState(fi, ST_L2_4);
875         }
876 }
877
878 static inline void
879 enquiry_cr(struct PStack *st, u_char typ, u_char cr, u_char pf)
880 {
881         struct sk_buff *skb;
882         struct Layer2 *l2;
883         u_char tmp[MAX_HEADER_LEN];
884         int i;
885
886         l2 = &st->l2;
887         i = sethdraddr(l2, tmp, cr);
888         if (test_bit(FLG_MOD128, &l2->flag)) {
889                 tmp[i++] = typ;
890                 tmp[i++] = (l2->vr << 1) | (pf ? 1 : 0);
891         } else
892                 tmp[i++] = (l2->vr << 5) | typ | (pf ? 0x10 : 0);
893         if (!(skb = alloc_skb(i, GFP_ATOMIC))) {
894                 printk(KERN_WARNING "isdl2 can't alloc sbbuff for enquiry_cr\n");
895                 return;
896         }
897         memcpy(skb_put(skb, i), tmp, i);
898         enqueue_super(st, skb);
899 }
900
901 static inline void
902 enquiry_response(struct PStack *st)
903 {
904         if (test_bit(FLG_OWN_BUSY, &st->l2.flag))
905                 enquiry_cr(st, RNR, RSP, 1);
906         else
907                 enquiry_cr(st, RR, RSP, 1);
908         test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag);
909 }
910
911 static inline void
912 transmit_enquiry(struct PStack *st)
913 {
914         if (test_bit(FLG_OWN_BUSY, &st->l2.flag))
915                 enquiry_cr(st, RNR, CMD, 1);
916         else
917                 enquiry_cr(st, RR, CMD, 1);
918         test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag);
919         start_t200(st, 9);
920 }
921
922
923 static void
924 nrerrorrecovery(struct FsmInst *fi)
925 {
926         struct PStack *st = fi->userdata;
927
928         st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'J');
929         establishlink(fi);
930         test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
931 }
932
933 static void
934 invoke_retransmission(struct PStack *st, unsigned int nr)
935 {
936         struct Layer2 *l2 = &st->l2;
937         u_int p1;
938         u_long flags;
939
940         spin_lock_irqsave(&l2->lock, flags);
941         if (l2->vs != nr) {
942                 while (l2->vs != nr) {
943                         (l2->vs)--;
944                         if (test_bit(FLG_MOD128, &l2->flag)) {
945                                 l2->vs %= 128;
946                                 p1 = (l2->vs - l2->va) % 128;
947                         } else {
948                                 l2->vs %= 8;
949                                 p1 = (l2->vs - l2->va) % 8;
950                         }
951                         p1 = (p1 + l2->sow) % l2->window;
952                         if (test_bit(FLG_LAPB, &l2->flag))
953                                 st->l1.bcs->tx_cnt += l2->windowar[p1]->len + l2headersize(l2, 0);
954                         skb_queue_head(&l2->i_queue, l2->windowar[p1]);
955                         l2->windowar[p1] = NULL;
956                 }
957                 spin_unlock_irqrestore(&l2->lock, flags);
958                 st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
959                 return;
960         }
961         spin_unlock_irqrestore(&l2->lock, flags);
962 }
963
964 static void
965 l2_st7_got_super(struct FsmInst *fi, int event, void *arg)
966 {
967         struct PStack *st = fi->userdata;
968         struct sk_buff *skb = arg;
969         int PollFlag, rsp, typ = RR;
970         unsigned int nr;
971         struct Layer2 *l2 = &st->l2;
972
973         rsp = *skb->data & 0x2;
974         if (test_bit(FLG_ORIG, &l2->flag))
975                 rsp = !rsp;
976
977         skb_pull(skb, l2addrsize(l2));
978         if (IsRNR(skb->data, st)) {
979                 set_peer_busy(l2);
980                 typ = RNR;
981         } else
982                 clear_peer_busy(l2);
983         if (IsREJ(skb->data, st))
984                 typ = REJ;
985
986         if (test_bit(FLG_MOD128, &l2->flag)) {
987                 PollFlag = (skb->data[1] & 0x1) == 0x1;
988                 nr = skb->data[1] >> 1;
989         } else {
990                 PollFlag = (skb->data[0] & 0x10);
991                 nr = (skb->data[0] >> 5) & 0x7;
992         }
993         dev_kfree_skb(skb);
994
995         if (PollFlag) {
996                 if (rsp)
997                         st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'A');
998                 else
999                         enquiry_response(st);
1000         }
1001         if (legalnr(st, nr)) {
1002                 if (typ == REJ) {
1003                         setva(st, nr);
1004                         invoke_retransmission(st, nr);
1005                         stop_t200(st, 10);
1006                         if (FsmAddTimer(&st->l2.t203, st->l2.T203,
1007                                         EV_L2_T203, NULL, 6))
1008                                 l2m_debug(&st->l2.l2m, "Restart T203 ST7 REJ");
1009                 } else if ((nr == l2->vs) && (typ == RR)) {
1010                         setva(st, nr);
1011                         stop_t200(st, 11);
1012                         FsmRestartTimer(&st->l2.t203, st->l2.T203,
1013                                         EV_L2_T203, NULL, 7);
1014                 } else if ((l2->va != nr) || (typ == RNR)) {
1015                         setva(st, nr);
1016                         if (typ != RR) FsmDelTimer(&st->l2.t203, 9);
1017                         restart_t200(st, 12);
1018                 }
1019                 if (!skb_queue_empty(&st->l2.i_queue) && (typ == RR))
1020                         st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
1021         } else
1022                 nrerrorrecovery(fi);
1023 }
1024
1025 static void
1026 l2_feed_i_if_reest(struct FsmInst *fi, int event, void *arg)
1027 {
1028         struct PStack *st = fi->userdata;
1029         struct sk_buff *skb = arg;
1030
1031         if (test_bit(FLG_LAPB, &st->l2.flag))
1032                 st->l1.bcs->tx_cnt += skb->len + l2headersize(&st->l2, 0);
1033         if (!test_bit(FLG_L3_INIT, &st->l2.flag))
1034                 skb_queue_tail(&st->l2.i_queue, skb);
1035         else
1036                 dev_kfree_skb(skb);
1037 }
1038
1039 static void
1040 l2_feed_i_pull(struct FsmInst *fi, int event, void *arg)
1041 {
1042         struct PStack *st = fi->userdata;
1043         struct sk_buff *skb = arg;
1044
1045         if (test_bit(FLG_LAPB, &st->l2.flag))
1046                 st->l1.bcs->tx_cnt += skb->len + l2headersize(&st->l2, 0);
1047         skb_queue_tail(&st->l2.i_queue, skb);
1048         st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
1049 }
1050
1051 static void
1052 l2_feed_iqueue(struct FsmInst *fi, int event, void *arg)
1053 {
1054         struct PStack *st = fi->userdata;
1055         struct sk_buff *skb = arg;
1056
1057         if (test_bit(FLG_LAPB, &st->l2.flag))
1058                 st->l1.bcs->tx_cnt += skb->len + l2headersize(&st->l2, 0);
1059         skb_queue_tail(&st->l2.i_queue, skb);
1060 }
1061
1062 static void
1063 l2_got_iframe(struct FsmInst *fi, int event, void *arg)
1064 {
1065         struct PStack *st = fi->userdata;
1066         struct sk_buff *skb = arg;
1067         struct Layer2 *l2 = &(st->l2);
1068         int PollFlag, ns, i;
1069         unsigned int nr;
1070
1071         i = l2addrsize(l2);
1072         if (test_bit(FLG_MOD128, &l2->flag)) {
1073                 PollFlag = ((skb->data[i + 1] & 0x1) == 0x1);
1074                 ns = skb->data[i] >> 1;
1075                 nr = (skb->data[i + 1] >> 1) & 0x7f;
1076         } else {
1077                 PollFlag = (skb->data[i] & 0x10);
1078                 ns = (skb->data[i] >> 1) & 0x7;
1079                 nr = (skb->data[i] >> 5) & 0x7;
1080         }
1081         if (test_bit(FLG_OWN_BUSY, &l2->flag)) {
1082                 dev_kfree_skb(skb);
1083                 if (PollFlag) enquiry_response(st);
1084         } else if (l2->vr == ns) {
1085                 (l2->vr)++;
1086                 if (test_bit(FLG_MOD128, &l2->flag))
1087                         l2->vr %= 128;
1088                 else
1089                         l2->vr %= 8;
1090                 test_and_clear_bit(FLG_REJEXC, &l2->flag);
1091
1092                 if (PollFlag)
1093                         enquiry_response(st);
1094                 else
1095                         test_and_set_bit(FLG_ACK_PEND, &l2->flag);
1096                 skb_pull(skb, l2headersize(l2, 0));
1097                 st->l2.l2l3(st, DL_DATA | INDICATION, skb);
1098         } else {
1099                 /* n(s)!=v(r) */
1100                 dev_kfree_skb(skb);
1101                 if (test_and_set_bit(FLG_REJEXC, &l2->flag)) {
1102                         if (PollFlag)
1103                                 enquiry_response(st);
1104                 } else {
1105                         enquiry_cr(st, REJ, RSP, PollFlag);
1106                         test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1107                 }
1108         }
1109
1110         if (legalnr(st, nr)) {
1111                 if (!test_bit(FLG_PEER_BUSY, &st->l2.flag) && (fi->state == ST_L2_7)) {
1112                         if (nr == st->l2.vs) {
1113                                 stop_t200(st, 13);
1114                                 FsmRestartTimer(&st->l2.t203, st->l2.T203,
1115                                                 EV_L2_T203, NULL, 7);
1116                         } else if (nr != st->l2.va)
1117                                 restart_t200(st, 14);
1118                 }
1119                 setva(st, nr);
1120         } else {
1121                 nrerrorrecovery(fi);
1122                 return;
1123         }
1124
1125         if (!skb_queue_empty(&st->l2.i_queue) && (fi->state == ST_L2_7))
1126                 st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
1127         if (test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag))
1128                 enquiry_cr(st, RR, RSP, 0);
1129 }
1130
1131 static void
1132 l2_got_tei(struct FsmInst *fi, int event, void *arg)
1133 {
1134         struct PStack *st = fi->userdata;
1135
1136         st->l2.tei = (long) arg;
1137
1138         if (fi->state == ST_L2_3) {
1139                 establishlink(fi);
1140                 test_and_set_bit(FLG_L3_INIT, &st->l2.flag);
1141         } else
1142                 FsmChangeState(fi, ST_L2_4);
1143         if (!skb_queue_empty(&st->l2.ui_queue))
1144                 tx_ui(st);
1145 }
1146
1147 static void
1148 l2_st5_tout_200(struct FsmInst *fi, int event, void *arg)
1149 {
1150         struct PStack *st = fi->userdata;
1151
1152         if (test_bit(FLG_LAPD, &st->l2.flag) &&
1153             test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
1154                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 9);
1155         } else if (st->l2.rc == st->l2.N200) {
1156                 FsmChangeState(fi, ST_L2_4);
1157                 test_and_clear_bit(FLG_T200_RUN, &st->l2.flag);
1158                 skb_queue_purge(&st->l2.i_queue);
1159                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'G');
1160                 if (test_bit(FLG_LAPB, &st->l2.flag))
1161                         st->l2.l2l1(st, PH_DEACTIVATE | REQUEST, NULL);
1162                 st5_dl_release_l2l3(st);
1163         } else {
1164                 st->l2.rc++;
1165                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 9);
1166                 send_uframe(st, (test_bit(FLG_MOD128, &st->l2.flag) ? SABME : SABM)
1167                             | 0x10, CMD);
1168         }
1169 }
1170
1171 static void
1172 l2_st6_tout_200(struct FsmInst *fi, int event, void *arg)
1173 {
1174         struct PStack *st = fi->userdata;
1175
1176         if (test_bit(FLG_LAPD, &st->l2.flag) &&
1177             test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
1178                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 9);
1179         } else if (st->l2.rc == st->l2.N200) {
1180                 FsmChangeState(fi, ST_L2_4);
1181                 test_and_clear_bit(FLG_T200_RUN, &st->l2.flag);
1182                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'H');
1183                 lapb_dl_release_l2l3(st, CONFIRM);
1184         } else {
1185                 st->l2.rc++;
1186                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200,
1187                             NULL, 9);
1188                 send_uframe(st, DISC | 0x10, CMD);
1189         }
1190 }
1191
1192 static void
1193 l2_st7_tout_200(struct FsmInst *fi, int event, void *arg)
1194 {
1195         struct PStack *st = fi->userdata;
1196
1197         if (test_bit(FLG_LAPD, &st->l2.flag) &&
1198             test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
1199                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 9);
1200                 return;
1201         }
1202         test_and_clear_bit(FLG_T200_RUN, &st->l2.flag);
1203         st->l2.rc = 0;
1204         FsmChangeState(fi, ST_L2_8);
1205
1206         transmit_enquiry(st);
1207         st->l2.rc++;
1208 }
1209
1210 static void
1211 l2_st8_tout_200(struct FsmInst *fi, int event, void *arg)
1212 {
1213         struct PStack *st = fi->userdata;
1214
1215         if (test_bit(FLG_LAPD, &st->l2.flag) &&
1216             test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
1217                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 9);
1218                 return;
1219         }
1220         test_and_clear_bit(FLG_T200_RUN, &st->l2.flag);
1221         if (st->l2.rc == st->l2.N200) {
1222                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'I');
1223                 establishlink(fi);
1224                 test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
1225         } else {
1226                 transmit_enquiry(st);
1227                 st->l2.rc++;
1228         }
1229 }
1230
1231 static void
1232 l2_st7_tout_203(struct FsmInst *fi, int event, void *arg)
1233 {
1234         struct PStack *st = fi->userdata;
1235
1236         if (test_bit(FLG_LAPD, &st->l2.flag) &&
1237             test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
1238                 FsmAddTimer(&st->l2.t203, st->l2.T203, EV_L2_T203, NULL, 9);
1239                 return;
1240         }
1241         FsmChangeState(fi, ST_L2_8);
1242         transmit_enquiry(st);
1243         st->l2.rc = 0;
1244 }
1245
1246 static void
1247 l2_pull_iqueue(struct FsmInst *fi, int event, void *arg)
1248 {
1249         struct PStack *st = fi->userdata;
1250         struct sk_buff *skb, *nskb;
1251         struct Layer2 *l2 = &st->l2;
1252         u_char header[MAX_HEADER_LEN];
1253         int i, hdr_space_needed;
1254         int unsigned p1;
1255         u_long flags;
1256
1257         if (!cansend(st))
1258                 return;
1259
1260         skb = skb_dequeue(&l2->i_queue);
1261         if (!skb)
1262                 return;
1263
1264         hdr_space_needed = l2headersize(l2, 0);
1265         nskb = skb_realloc_headroom(skb, hdr_space_needed);
1266         if (!nskb) {
1267                 skb_queue_head(&l2->i_queue, skb);
1268                 return;
1269         }
1270         spin_lock_irqsave(&l2->lock, flags);
1271         if (test_bit(FLG_MOD128, &l2->flag))
1272                 p1 = (l2->vs - l2->va) % 128;
1273         else
1274                 p1 = (l2->vs - l2->va) % 8;
1275         p1 = (p1 + l2->sow) % l2->window;
1276         if (l2->windowar[p1]) {
1277                 printk(KERN_WARNING "isdnl2 try overwrite ack queue entry %d\n",
1278                        p1);
1279                 dev_kfree_skb(l2->windowar[p1]);
1280         }
1281         l2->windowar[p1] = skb;
1282
1283         i = sethdraddr(&st->l2, header, CMD);
1284
1285         if (test_bit(FLG_MOD128, &l2->flag)) {
1286                 header[i++] = l2->vs << 1;
1287                 header[i++] = l2->vr << 1;
1288                 l2->vs = (l2->vs + 1) % 128;
1289         } else {
1290                 header[i++] = (l2->vr << 5) | (l2->vs << 1);
1291                 l2->vs = (l2->vs + 1) % 8;
1292         }
1293         spin_unlock_irqrestore(&l2->lock, flags);
1294         memcpy(skb_push(nskb, i), header, i);
1295         st->l2.l2l1(st, PH_PULL | INDICATION, nskb);
1296         test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag);
1297         if (!test_and_set_bit(FLG_T200_RUN, &st->l2.flag)) {
1298                 FsmDelTimer(&st->l2.t203, 13);
1299                 FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 11);
1300         }
1301         if (!skb_queue_empty(&l2->i_queue) && cansend(st))
1302                 st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
1303 }
1304
1305 static void
1306 l2_st8_got_super(struct FsmInst *fi, int event, void *arg)
1307 {
1308         struct PStack *st = fi->userdata;
1309         struct sk_buff *skb = arg;
1310         int PollFlag, rsp, rnr = 0;
1311         unsigned int nr;
1312         struct Layer2 *l2 = &st->l2;
1313
1314         rsp = *skb->data & 0x2;
1315         if (test_bit(FLG_ORIG, &l2->flag))
1316                 rsp = !rsp;
1317
1318         skb_pull(skb, l2addrsize(l2));
1319
1320         if (IsRNR(skb->data, st)) {
1321                 set_peer_busy(l2);
1322                 rnr = 1;
1323         } else
1324                 clear_peer_busy(l2);
1325
1326         if (test_bit(FLG_MOD128, &l2->flag)) {
1327                 PollFlag = (skb->data[1] & 0x1) == 0x1;
1328                 nr = skb->data[1] >> 1;
1329         } else {
1330                 PollFlag = (skb->data[0] & 0x10);
1331                 nr = (skb->data[0] >> 5) & 0x7;
1332         }
1333         dev_kfree_skb(skb);
1334
1335         if (rsp && PollFlag) {
1336                 if (legalnr(st, nr)) {
1337                         if (rnr) {
1338                                 restart_t200(st, 15);
1339                         } else {
1340                                 stop_t200(st, 16);
1341                                 FsmAddTimer(&l2->t203, l2->T203,
1342                                             EV_L2_T203, NULL, 5);
1343                                 setva(st, nr);
1344                         }
1345                         invoke_retransmission(st, nr);
1346                         FsmChangeState(fi, ST_L2_7);
1347                         if (!skb_queue_empty(&l2->i_queue) && cansend(st))
1348                                 st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
1349                 } else
1350                         nrerrorrecovery(fi);
1351         } else {
1352                 if (!rsp && PollFlag)
1353                         enquiry_response(st);
1354                 if (legalnr(st, nr)) {
1355                         setva(st, nr);
1356                 } else
1357                         nrerrorrecovery(fi);
1358         }
1359 }
1360
1361 static void
1362 l2_got_FRMR(struct FsmInst *fi, int event, void *arg)
1363 {
1364         struct PStack *st = fi->userdata;
1365         struct sk_buff *skb = arg;
1366
1367         skb_pull(skb, l2addrsize(&st->l2) + 1);
1368
1369         if (!(skb->data[0] & 1) || ((skb->data[0] & 3) == 1) ||         /* I or S */
1370             (IsUA(skb->data) && (fi->state == ST_L2_7))) {
1371                 st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'K');
1372                 establishlink(fi);
1373                 test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
1374         }
1375         dev_kfree_skb(skb);
1376 }
1377
1378 static void
1379 l2_st24_tei_remove(struct FsmInst *fi, int event, void *arg)
1380 {
1381         struct PStack *st = fi->userdata;
1382
1383         skb_queue_purge(&st->l2.ui_queue);
1384         st->l2.tei = -1;
1385         FsmChangeState(fi, ST_L2_1);
1386 }
1387
1388 static void
1389 l2_st3_tei_remove(struct FsmInst *fi, int event, void *arg)
1390 {
1391         struct PStack *st = fi->userdata;
1392
1393         skb_queue_purge(&st->l2.ui_queue);
1394         st->l2.tei = -1;
1395         st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
1396         FsmChangeState(fi, ST_L2_1);
1397 }
1398
1399 static void
1400 l2_st5_tei_remove(struct FsmInst *fi, int event, void *arg)
1401 {
1402         struct PStack *st = fi->userdata;
1403
1404         skb_queue_purge(&st->l2.i_queue);
1405         skb_queue_purge(&st->l2.ui_queue);
1406         freewin(st);
1407         st->l2.tei = -1;
1408         stop_t200(st, 17);
1409         st5_dl_release_l2l3(st);
1410         FsmChangeState(fi, ST_L2_1);
1411 }
1412
1413 static void
1414 l2_st6_tei_remove(struct FsmInst *fi, int event, void *arg)
1415 {
1416         struct PStack *st = fi->userdata;
1417
1418         skb_queue_purge(&st->l2.ui_queue);
1419         st->l2.tei = -1;
1420         stop_t200(st, 18);
1421         st->l2.l2l3(st, DL_RELEASE | CONFIRM, NULL);
1422         FsmChangeState(fi, ST_L2_1);
1423 }
1424
1425 static void
1426 l2_tei_remove(struct FsmInst *fi, int event, void *arg)
1427 {
1428         struct PStack *st = fi->userdata;
1429
1430         skb_queue_purge(&st->l2.i_queue);
1431         skb_queue_purge(&st->l2.ui_queue);
1432         freewin(st);
1433         st->l2.tei = -1;
1434         stop_t200(st, 17);
1435         FsmDelTimer(&st->l2.t203, 19);
1436         st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
1437         FsmChangeState(fi, ST_L2_1);
1438 }
1439
1440 static void
1441 l2_st14_persistent_da(struct FsmInst *fi, int event, void *arg)
1442 {
1443         struct PStack *st = fi->userdata;
1444
1445         skb_queue_purge(&st->l2.i_queue);
1446         skb_queue_purge(&st->l2.ui_queue);
1447         if (test_and_clear_bit(FLG_ESTAB_PEND, &st->l2.flag))
1448                 st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
1449 }
1450
1451 static void
1452 l2_st5_persistent_da(struct FsmInst *fi, int event, void *arg)
1453 {
1454         struct PStack *st = fi->userdata;
1455
1456         skb_queue_purge(&st->l2.i_queue);
1457         skb_queue_purge(&st->l2.ui_queue);
1458         freewin(st);
1459         stop_t200(st, 19);
1460         st5_dl_release_l2l3(st);
1461         FsmChangeState(fi, ST_L2_4);
1462 }
1463
1464 static void
1465 l2_st6_persistent_da(struct FsmInst *fi, int event, void *arg)
1466 {
1467         struct PStack *st = fi->userdata;
1468
1469         skb_queue_purge(&st->l2.ui_queue);
1470         stop_t200(st, 20);
1471         st->l2.l2l3(st, DL_RELEASE | CONFIRM, NULL);
1472         FsmChangeState(fi, ST_L2_4);
1473 }
1474
1475 static void
1476 l2_persistent_da(struct FsmInst *fi, int event, void *arg)
1477 {
1478         struct PStack *st = fi->userdata;
1479
1480         skb_queue_purge(&st->l2.i_queue);
1481         skb_queue_purge(&st->l2.ui_queue);
1482         freewin(st);
1483         stop_t200(st, 19);
1484         FsmDelTimer(&st->l2.t203, 19);
1485         st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
1486         FsmChangeState(fi, ST_L2_4);
1487 }
1488
1489 static void
1490 l2_set_own_busy(struct FsmInst *fi, int event, void *arg)
1491 {
1492         struct PStack *st = fi->userdata;
1493
1494         if (!test_and_set_bit(FLG_OWN_BUSY, &st->l2.flag)) {
1495                 enquiry_cr(st, RNR, RSP, 0);
1496                 test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag);
1497         }
1498 }
1499
1500 static void
1501 l2_clear_own_busy(struct FsmInst *fi, int event, void *arg)
1502 {
1503         struct PStack *st = fi->userdata;
1504
1505         if (!test_and_clear_bit(FLG_OWN_BUSY, &st->l2.flag)) {
1506                 enquiry_cr(st, RR, RSP, 0);
1507                 test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag);
1508         }
1509 }
1510
1511 static void
1512 l2_frame_error(struct FsmInst *fi, int event, void *arg)
1513 {
1514         struct PStack *st = fi->userdata;
1515
1516         st->ma.layer(st, MDL_ERROR | INDICATION, arg);
1517 }
1518
1519 static void
1520 l2_frame_error_reest(struct FsmInst *fi, int event, void *arg)
1521 {
1522         struct PStack *st = fi->userdata;
1523
1524         st->ma.layer(st, MDL_ERROR | INDICATION, arg);
1525         establishlink(fi);
1526         test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
1527 }
1528
1529 static struct FsmNode L2FnList[] __initdata =
1530 {
1531         {ST_L2_1, EV_L2_DL_ESTABLISH_REQ, l2_mdl_assign},
1532         {ST_L2_2, EV_L2_DL_ESTABLISH_REQ, l2_go_st3},
1533         {ST_L2_4, EV_L2_DL_ESTABLISH_REQ, l2_establish},
1534         {ST_L2_5, EV_L2_DL_ESTABLISH_REQ, l2_discard_i_setl3},
1535         {ST_L2_7, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish},
1536         {ST_L2_8, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish},
1537         {ST_L2_4, EV_L2_DL_RELEASE_REQ, l2_release},
1538         {ST_L2_5, EV_L2_DL_RELEASE_REQ, l2_pend_rel},
1539         {ST_L2_7, EV_L2_DL_RELEASE_REQ, l2_disconnect},
1540         {ST_L2_8, EV_L2_DL_RELEASE_REQ, l2_disconnect},
1541         {ST_L2_5, EV_L2_DL_DATA, l2_feed_i_if_reest},
1542         {ST_L2_7, EV_L2_DL_DATA, l2_feed_i_pull},
1543         {ST_L2_8, EV_L2_DL_DATA, l2_feed_iqueue},
1544         {ST_L2_1, EV_L2_DL_UNIT_DATA, l2_queue_ui_assign},
1545         {ST_L2_2, EV_L2_DL_UNIT_DATA, l2_queue_ui},
1546         {ST_L2_3, EV_L2_DL_UNIT_DATA, l2_queue_ui},
1547         {ST_L2_4, EV_L2_DL_UNIT_DATA, l2_send_ui},
1548         {ST_L2_5, EV_L2_DL_UNIT_DATA, l2_send_ui},
1549         {ST_L2_6, EV_L2_DL_UNIT_DATA, l2_send_ui},
1550         {ST_L2_7, EV_L2_DL_UNIT_DATA, l2_send_ui},
1551         {ST_L2_8, EV_L2_DL_UNIT_DATA, l2_send_ui},
1552         {ST_L2_1, EV_L2_MDL_ASSIGN, l2_got_tei},
1553         {ST_L2_2, EV_L2_MDL_ASSIGN, l2_got_tei},
1554         {ST_L2_3, EV_L2_MDL_ASSIGN, l2_got_tei},
1555         {ST_L2_2, EV_L2_MDL_ERROR, l2_st24_tei_remove},
1556         {ST_L2_3, EV_L2_MDL_ERROR, l2_st3_tei_remove},
1557         {ST_L2_4, EV_L2_MDL_REMOVE, l2_st24_tei_remove},
1558         {ST_L2_5, EV_L2_MDL_REMOVE, l2_st5_tei_remove},
1559         {ST_L2_6, EV_L2_MDL_REMOVE, l2_st6_tei_remove},
1560         {ST_L2_7, EV_L2_MDL_REMOVE, l2_tei_remove},
1561         {ST_L2_8, EV_L2_MDL_REMOVE, l2_tei_remove},
1562         {ST_L2_4, EV_L2_SABME, l2_start_multi},
1563         {ST_L2_5, EV_L2_SABME, l2_send_UA},
1564         {ST_L2_6, EV_L2_SABME, l2_send_DM},
1565         {ST_L2_7, EV_L2_SABME, l2_restart_multi},
1566         {ST_L2_8, EV_L2_SABME, l2_restart_multi},
1567         {ST_L2_4, EV_L2_DISC, l2_send_DM},
1568         {ST_L2_5, EV_L2_DISC, l2_send_DM},
1569         {ST_L2_6, EV_L2_DISC, l2_send_UA},
1570         {ST_L2_7, EV_L2_DISC, l2_stop_multi},
1571         {ST_L2_8, EV_L2_DISC, l2_stop_multi},
1572         {ST_L2_4, EV_L2_UA, l2_mdl_error_ua},
1573         {ST_L2_5, EV_L2_UA, l2_connected},
1574         {ST_L2_6, EV_L2_UA, l2_released},
1575         {ST_L2_7, EV_L2_UA, l2_mdl_error_ua},
1576         {ST_L2_8, EV_L2_UA, l2_mdl_error_ua},
1577         {ST_L2_4, EV_L2_DM, l2_reestablish},
1578         {ST_L2_5, EV_L2_DM, l2_st5_dm_release},
1579         {ST_L2_6, EV_L2_DM, l2_st6_dm_release},
1580         {ST_L2_7, EV_L2_DM, l2_mdl_error_dm},
1581         {ST_L2_8, EV_L2_DM, l2_st8_mdl_error_dm},
1582         {ST_L2_1, EV_L2_UI, l2_got_ui},
1583         {ST_L2_2, EV_L2_UI, l2_got_ui},
1584         {ST_L2_3, EV_L2_UI, l2_got_ui},
1585         {ST_L2_4, EV_L2_UI, l2_got_ui},
1586         {ST_L2_5, EV_L2_UI, l2_got_ui},
1587         {ST_L2_6, EV_L2_UI, l2_got_ui},
1588         {ST_L2_7, EV_L2_UI, l2_got_ui},
1589         {ST_L2_8, EV_L2_UI, l2_got_ui},
1590         {ST_L2_7, EV_L2_FRMR, l2_got_FRMR},
1591         {ST_L2_8, EV_L2_FRMR, l2_got_FRMR},
1592         {ST_L2_7, EV_L2_SUPER, l2_st7_got_super},
1593         {ST_L2_8, EV_L2_SUPER, l2_st8_got_super},
1594         {ST_L2_7, EV_L2_I, l2_got_iframe},
1595         {ST_L2_8, EV_L2_I, l2_got_iframe},
1596         {ST_L2_5, EV_L2_T200, l2_st5_tout_200},
1597         {ST_L2_6, EV_L2_T200, l2_st6_tout_200},
1598         {ST_L2_7, EV_L2_T200, l2_st7_tout_200},
1599         {ST_L2_8, EV_L2_T200, l2_st8_tout_200},
1600         {ST_L2_7, EV_L2_T203, l2_st7_tout_203},
1601         {ST_L2_7, EV_L2_ACK_PULL, l2_pull_iqueue},
1602         {ST_L2_7, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
1603         {ST_L2_8, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
1604         {ST_L2_7, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy},
1605         {ST_L2_8, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy},
1606         {ST_L2_4, EV_L2_FRAME_ERROR, l2_frame_error},
1607         {ST_L2_5, EV_L2_FRAME_ERROR, l2_frame_error},
1608         {ST_L2_6, EV_L2_FRAME_ERROR, l2_frame_error},
1609         {ST_L2_7, EV_L2_FRAME_ERROR, l2_frame_error_reest},
1610         {ST_L2_8, EV_L2_FRAME_ERROR, l2_frame_error_reest},
1611         {ST_L2_1, EV_L1_DEACTIVATE, l2_st14_persistent_da},
1612         {ST_L2_2, EV_L1_DEACTIVATE, l2_st24_tei_remove},
1613         {ST_L2_3, EV_L1_DEACTIVATE, l2_st3_tei_remove},
1614         {ST_L2_4, EV_L1_DEACTIVATE, l2_st14_persistent_da},
1615         {ST_L2_5, EV_L1_DEACTIVATE, l2_st5_persistent_da},
1616         {ST_L2_6, EV_L1_DEACTIVATE, l2_st6_persistent_da},
1617         {ST_L2_7, EV_L1_DEACTIVATE, l2_persistent_da},
1618         {ST_L2_8, EV_L1_DEACTIVATE, l2_persistent_da},
1619 };
1620
1621 static void
1622 isdnl2_l1l2(struct PStack *st, int pr, void *arg)
1623 {
1624         struct sk_buff *skb = arg;
1625         u_char *datap;
1626         int ret = 1, len;
1627         int c = 0;
1628
1629         switch (pr) {
1630         case (PH_DATA | INDICATION):
1631                 datap = skb->data;
1632                 len = l2addrsize(&st->l2);
1633                 if (skb->len > len)
1634                         datap += len;
1635                 else {
1636                         FsmEvent(&st->l2.l2m, EV_L2_FRAME_ERROR, (void *) 'N');
1637                         dev_kfree_skb(skb);
1638                         return;
1639                 }
1640                 if (!(*datap & 1)) {    /* I-Frame */
1641                         if (!(c = iframe_error(st, skb)))
1642                                 ret = FsmEvent(&st->l2.l2m, EV_L2_I, skb);
1643                 } else if (IsSFrame(datap, st)) {       /* S-Frame */
1644                         if (!(c = super_error(st, skb)))
1645                                 ret = FsmEvent(&st->l2.l2m, EV_L2_SUPER, skb);
1646                 } else if (IsUI(datap)) {
1647                         if (!(c = UI_error(st, skb)))
1648                                 ret = FsmEvent(&st->l2.l2m, EV_L2_UI, skb);
1649                 } else if (IsSABME(datap, st)) {
1650                         if (!(c = unnum_error(st, skb, CMD)))
1651                                 ret = FsmEvent(&st->l2.l2m, EV_L2_SABME, skb);
1652                 } else if (IsUA(datap)) {
1653                         if (!(c = unnum_error(st, skb, RSP)))
1654                                 ret = FsmEvent(&st->l2.l2m, EV_L2_UA, skb);
1655                 } else if (IsDISC(datap)) {
1656                         if (!(c = unnum_error(st, skb, CMD)))
1657                                 ret = FsmEvent(&st->l2.l2m, EV_L2_DISC, skb);
1658                 } else if (IsDM(datap)) {
1659                         if (!(c = unnum_error(st, skb, RSP)))
1660                                 ret = FsmEvent(&st->l2.l2m, EV_L2_DM, skb);
1661                 } else if (IsFRMR(datap)) {
1662                         if (!(c = FRMR_error(st, skb)))
1663                                 ret = FsmEvent(&st->l2.l2m, EV_L2_FRMR, skb);
1664                 } else {
1665                         FsmEvent(&st->l2.l2m, EV_L2_FRAME_ERROR, (void *) 'L');
1666                         dev_kfree_skb(skb);
1667                         ret = 0;
1668                 }
1669                 if (c) {
1670                         dev_kfree_skb(skb);
1671                         FsmEvent(&st->l2.l2m, EV_L2_FRAME_ERROR, (void *)(long)c);
1672                         ret = 0;
1673                 }
1674                 if (ret)
1675                         dev_kfree_skb(skb);
1676                 break;
1677         case (PH_PULL | CONFIRM):
1678                 FsmEvent(&st->l2.l2m, EV_L2_ACK_PULL, arg);
1679                 break;
1680         case (PH_PAUSE | INDICATION):
1681                 test_and_set_bit(FLG_DCHAN_BUSY, &st->l2.flag);
1682                 break;
1683         case (PH_PAUSE | CONFIRM):
1684                 test_and_clear_bit(FLG_DCHAN_BUSY, &st->l2.flag);
1685                 break;
1686         case (PH_ACTIVATE | CONFIRM):
1687         case (PH_ACTIVATE | INDICATION):
1688                 test_and_set_bit(FLG_L1_ACTIV, &st->l2.flag);
1689                 if (test_and_clear_bit(FLG_ESTAB_PEND, &st->l2.flag))
1690                         FsmEvent(&st->l2.l2m, EV_L2_DL_ESTABLISH_REQ, arg);
1691                 break;
1692         case (PH_DEACTIVATE | INDICATION):
1693         case (PH_DEACTIVATE | CONFIRM):
1694                 test_and_clear_bit(FLG_L1_ACTIV, &st->l2.flag);
1695                 FsmEvent(&st->l2.l2m, EV_L1_DEACTIVATE, arg);
1696                 break;
1697         default:
1698                 l2m_debug(&st->l2.l2m, "l2 unknown pr %04x", pr);
1699                 break;
1700         }
1701 }
1702
1703 static void
1704 isdnl2_l3l2(struct PStack *st, int pr, void *arg)
1705 {
1706         switch (pr) {
1707         case (DL_DATA | REQUEST):
1708                 if (FsmEvent(&st->l2.l2m, EV_L2_DL_DATA, arg)) {
1709                         dev_kfree_skb((struct sk_buff *) arg);
1710                 }
1711                 break;
1712         case (DL_UNIT_DATA | REQUEST):
1713                 if (FsmEvent(&st->l2.l2m, EV_L2_DL_UNIT_DATA, arg)) {
1714                         dev_kfree_skb((struct sk_buff *) arg);
1715                 }
1716                 break;
1717         case (DL_ESTABLISH | REQUEST):
1718                 if (test_bit(FLG_L1_ACTIV, &st->l2.flag)) {
1719                         if (test_bit(FLG_LAPD, &st->l2.flag) ||
1720                             test_bit(FLG_ORIG, &st->l2.flag)) {
1721                                 FsmEvent(&st->l2.l2m, EV_L2_DL_ESTABLISH_REQ, arg);
1722                         }
1723                 } else {
1724                         if (test_bit(FLG_LAPD, &st->l2.flag) ||
1725                             test_bit(FLG_ORIG, &st->l2.flag)) {
1726                                 test_and_set_bit(FLG_ESTAB_PEND, &st->l2.flag);
1727                         }
1728                         st->l2.l2l1(st, PH_ACTIVATE, NULL);
1729                 }
1730                 break;
1731         case (DL_RELEASE | REQUEST):
1732                 if (test_bit(FLG_LAPB, &st->l2.flag)) {
1733                         st->l2.l2l1(st, PH_DEACTIVATE, NULL);
1734                 }
1735                 FsmEvent(&st->l2.l2m, EV_L2_DL_RELEASE_REQ, arg);
1736                 break;
1737         case (MDL_ASSIGN | REQUEST):
1738                 FsmEvent(&st->l2.l2m, EV_L2_MDL_ASSIGN, arg);
1739                 break;
1740         case (MDL_REMOVE | REQUEST):
1741                 FsmEvent(&st->l2.l2m, EV_L2_MDL_REMOVE, arg);
1742                 break;
1743         case (MDL_ERROR | RESPONSE):
1744                 FsmEvent(&st->l2.l2m, EV_L2_MDL_ERROR, arg);
1745                 break;
1746         }
1747 }
1748
1749 void
1750 releasestack_isdnl2(struct PStack *st)
1751 {
1752         FsmDelTimer(&st->l2.t200, 21);
1753         FsmDelTimer(&st->l2.t203, 16);
1754         skb_queue_purge(&st->l2.i_queue);
1755         skb_queue_purge(&st->l2.ui_queue);
1756         ReleaseWin(&st->l2);
1757 }
1758
1759 static void
1760 l2m_debug(struct FsmInst *fi, char *fmt, ...)
1761 {
1762         va_list args;
1763         struct PStack *st = fi->userdata;
1764
1765         va_start(args, fmt);
1766         VHiSax_putstatus(st->l1.hardware, st->l2.debug_id, fmt, args);
1767         va_end(args);
1768 }
1769
1770 void
1771 setstack_isdnl2(struct PStack *st, char *debug_id)
1772 {
1773         spin_lock_init(&st->l2.lock);
1774         st->l1.l1l2 = isdnl2_l1l2;
1775         st->l3.l3l2 = isdnl2_l3l2;
1776
1777         skb_queue_head_init(&st->l2.i_queue);
1778         skb_queue_head_init(&st->l2.ui_queue);
1779         InitWin(&st->l2);
1780         st->l2.debug = 0;
1781
1782         st->l2.l2m.fsm = &l2fsm;
1783         if (test_bit(FLG_LAPB, &st->l2.flag))
1784                 st->l2.l2m.state = ST_L2_4;
1785         else
1786                 st->l2.l2m.state = ST_L2_1;
1787         st->l2.l2m.debug = 0;
1788         st->l2.l2m.userdata = st;
1789         st->l2.l2m.userint = 0;
1790         st->l2.l2m.printdebug = l2m_debug;
1791         strcpy(st->l2.debug_id, debug_id);
1792
1793         FsmInitTimer(&st->l2.l2m, &st->l2.t200);
1794         FsmInitTimer(&st->l2.l2m, &st->l2.t203);
1795 }
1796
1797 static void
1798 transl2_l3l2(struct PStack *st, int pr, void *arg)
1799 {
1800         switch (pr) {
1801         case (DL_DATA | REQUEST):
1802         case (DL_UNIT_DATA | REQUEST):
1803                 st->l2.l2l1(st, PH_DATA | REQUEST, arg);
1804                 break;
1805         case (DL_ESTABLISH | REQUEST):
1806                 st->l2.l2l1(st, PH_ACTIVATE | REQUEST, NULL);
1807                 break;
1808         case (DL_RELEASE | REQUEST):
1809                 st->l2.l2l1(st, PH_DEACTIVATE | REQUEST, NULL);
1810                 break;
1811         }
1812 }
1813
1814 void
1815 setstack_transl2(struct PStack *st)
1816 {
1817         st->l3.l3l2 = transl2_l3l2;
1818 }
1819
1820 void
1821 releasestack_transl2(struct PStack *st)
1822 {
1823 }
1824
1825 int __init
1826 Isdnl2New(void)
1827 {
1828         l2fsm.state_count = L2_STATE_COUNT;
1829         l2fsm.event_count = L2_EVENT_COUNT;
1830         l2fsm.strEvent = strL2Event;
1831         l2fsm.strState = strL2State;
1832         return FsmNew(&l2fsm, L2FnList, ARRAY_SIZE(L2FnList));
1833 }
1834
1835 void
1836 Isdnl2Free(void)
1837 {
1838         FsmFree(&l2fsm);
1839 }