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