drivers/isdn/mISDN/layer2.c

   1 /*
   2  *
   3  * Author       Karsten Keil <kkeil@novell.com>
   4  *
   5  * Copyright 2008  by Karsten Keil <kkeil@novell.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  */
  17
  18 #include <linux/mISDNif.h>
  19 #include <linux/slab.h>
  20 #include "core.h"
  21 #include "fsm.h"
  22 #include "layer2.h"
  23
  24 static u_int *debug;
  25
  26 static
  27 struct Fsm l2fsm = {NULL, 0, 0, NULL, NULL};
  28
  29 static char *strL2State[] =
  30 {
  31         "ST_L2_1",
  32         "ST_L2_2",
  33         "ST_L2_3",
  34         "ST_L2_4",
  35         "ST_L2_5",
  36         "ST_L2_6",
  37         "ST_L2_7",
  38         "ST_L2_8",
  39 };
  40
  41 enum {
  42         EV_L2_UI,
  43         EV_L2_SABME,
  44         EV_L2_DISC,
  45         EV_L2_DM,
  46         EV_L2_UA,
  47         EV_L2_FRMR,
  48         EV_L2_SUPER,
  49         EV_L2_I,
  50         EV_L2_DL_DATA,
  51         EV_L2_ACK_PULL,
  52         EV_L2_DL_UNITDATA,
  53         EV_L2_DL_ESTABLISH_REQ,
  54         EV_L2_DL_RELEASE_REQ,
  55         EV_L2_MDL_ASSIGN,
  56         EV_L2_MDL_REMOVE,
  57         EV_L2_MDL_ERROR,
  58         EV_L1_DEACTIVATE,
  59         EV_L2_T200,
  60         EV_L2_T203,
  61         EV_L2_T200I,
  62         EV_L2_T203I,
  63         EV_L2_SET_OWN_BUSY,
  64         EV_L2_CLEAR_OWN_BUSY,
  65         EV_L2_FRAME_ERROR,
  66 };
  67
  68 #define L2_EVENT_COUNT (EV_L2_FRAME_ERROR + 1)
  69
  70 static char *strL2Event[] =
  71 {
  72         "EV_L2_UI",
  73         "EV_L2_SABME",
  74         "EV_L2_DISC",
  75         "EV_L2_DM",
  76         "EV_L2_UA",
  77         "EV_L2_FRMR",
  78         "EV_L2_SUPER",
  79         "EV_L2_I",
  80         "EV_L2_DL_DATA",
  81         "EV_L2_ACK_PULL",
  82         "EV_L2_DL_UNITDATA",
  83         "EV_L2_DL_ESTABLISH_REQ",
  84         "EV_L2_DL_RELEASE_REQ",
  85         "EV_L2_MDL_ASSIGN",
  86         "EV_L2_MDL_REMOVE",
  87         "EV_L2_MDL_ERROR",
  88         "EV_L1_DEACTIVATE",
  89         "EV_L2_T200",
  90         "EV_L2_T203",
  91         "EV_L2_T200I",
  92         "EV_L2_T203I",
  93         "EV_L2_SET_OWN_BUSY",
  94         "EV_L2_CLEAR_OWN_BUSY",
  95         "EV_L2_FRAME_ERROR",
  96 };
  97
  98 static void
  99 l2m_debug(struct FsmInst *fi, char *fmt, ...)
 100 {
 101         struct layer2 *l2 = fi->userdata;
 102         struct va_format vaf;
 103         va_list va;
 104
 105         if (!(*debug & DEBUG_L2_FSM))
 106                 return;
 107
 108         va_start(va, fmt);
 109
 110         vaf.fmt = fmt;
 111         vaf.va = &va;
 112
 113         printk(KERN_DEBUG "%s l2 (sapi %d tei %d): %pV\n",
 114                mISDNDevName4ch(&l2->ch), l2->sapi, l2->tei, &vaf);
 115
 116         va_end(va);
 117 }
 118
 119 inline u_int
 120 l2headersize(struct layer2 *l2, int ui)
 121 {
 122         return ((test_bit(FLG_MOD128, &l2->flag) && (!ui)) ? 2 : 1) +
 123                 (test_bit(FLG_LAPD, &l2->flag) ? 2 : 1);
 124 }
 125
 126 inline u_int
 127 l2addrsize(struct layer2 *l2)
 128 {
 129         return test_bit(FLG_LAPD, &l2->flag) ? 2 : 1;
 130 }
 131
 132 static u_int
 133 l2_newid(struct layer2 *l2)
 134 {
 135         u_int   id;
 136
 137         id = l2->next_id++;
 138         if (id == 0x7fff)
 139                 l2->next_id = 1;
 140         id <<= 16;
 141         id |= l2->tei << 8;
 142         id |= l2->sapi;
 143         return id;
 144 }
 145
 146 static void
 147 l2up(struct layer2 *l2, u_int prim, struct sk_buff *skb)
 148 {
 149         int     err;
 150
 151         if (!l2->up)
 152                 return;
 153         mISDN_HEAD_PRIM(skb) = prim;
 154         mISDN_HEAD_ID(skb) = (l2->ch.nr << 16) | l2->ch.addr;
 155         err = l2->up->send(l2->up, skb);
 156         if (err) {
 157                 printk(KERN_WARNING "%s: dev %s err=%d\n", __func__,
 158                        mISDNDevName4ch(&l2->ch), err);
 159                 dev_kfree_skb(skb);
 160         }
 161 }
 162
 163 static void
 164 l2up_create(struct layer2 *l2, u_int prim, int len, void *arg)
 165 {
 166         struct sk_buff  *skb;
 167         struct mISDNhead *hh;
 168         int             err;
 169
 170         if (!l2->up)
 171                 return;
 172         skb = mI_alloc_skb(len, GFP_ATOMIC);
 173         if (!skb)
 174                 return;
 175         hh = mISDN_HEAD_P(skb);
 176         hh->prim = prim;
 177         hh->id = (l2->ch.nr << 16) | l2->ch.addr;
 178         if (len)
 179                 memcpy(skb_put(skb, len), arg, len);
 180         err = l2->up->send(l2->up, skb);
 181         if (err) {
 182                 printk(KERN_WARNING "%s: dev %s err=%d\n", __func__,
 183                        mISDNDevName4ch(&l2->ch), err);
 184                 dev_kfree_skb(skb);
 185         }
 186 }
 187
 188 static int
 189 l2down_skb(struct layer2 *l2, struct sk_buff *skb) {
 190         int ret;
 191
 192         ret = l2->ch.recv(l2->ch.peer, skb);
 193         if (ret && (*debug & DEBUG_L2_RECV))
 194                 printk(KERN_DEBUG "l2down_skb: dev %s ret(%d)\n",
 195                        mISDNDevName4ch(&l2->ch), ret);
 196         return ret;
 197 }
 198
 199 static int
 200 l2down_raw(struct layer2 *l2, struct sk_buff *skb)
 201 {
 202         struct mISDNhead *hh = mISDN_HEAD_P(skb);
 203
 204         if (hh->prim == PH_DATA_REQ) {
 205                 if (test_and_set_bit(FLG_L1_NOTREADY, &l2->flag)) {
 206                         skb_queue_tail(&l2->down_queue, skb);
 207                         return 0;
 208                 }
 209                 l2->down_id = mISDN_HEAD_ID(skb);
 210         }
 211         return l2down_skb(l2, skb);
 212 }
 213
 214 static int
 215 l2down(struct layer2 *l2, u_int prim, u_int id, struct sk_buff *skb)
 216 {
 217         struct mISDNhead *hh = mISDN_HEAD_P(skb);
 218
 219         hh->prim = prim;
 220         hh->id = id;
 221         return l2down_raw(l2, skb);
 222 }
 223
 224 static int
 225 l2down_create(struct layer2 *l2, u_int prim, u_int id, int len, void *arg)
 226 {
 227         struct sk_buff  *skb;
 228         int             err;
 229         struct mISDNhead *hh;
 230
 231         skb = mI_alloc_skb(len, GFP_ATOMIC);
 232         if (!skb)
 233                 return -ENOMEM;
 234         hh = mISDN_HEAD_P(skb);
 235         hh->prim = prim;
 236         hh->id = id;
 237         if (len)
 238                 memcpy(skb_put(skb, len), arg, len);
 239         err = l2down_raw(l2, skb);
 240         if (err)
 241                 dev_kfree_skb(skb);
 242         return err;
 243 }
 244
 245 static int
 246 ph_data_confirm(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb) {
 247         struct sk_buff *nskb = skb;
 248         int ret = -EAGAIN;
 249
 250         if (test_bit(FLG_L1_NOTREADY, &l2->flag)) {
 251                 if (hh->id == l2->down_id) {
 252                         nskb = skb_dequeue(&l2->down_queue);
 253                         if (nskb) {
 254                                 l2->down_id = mISDN_HEAD_ID(nskb);
 255                                 if (l2down_skb(l2, nskb)) {
 256                                         dev_kfree_skb(nskb);
 257                                         l2->down_id = MISDN_ID_NONE;
 258                                 }
 259                         } else
 260                                 l2->down_id = MISDN_ID_NONE;
 261                         if (ret) {
 262                                 dev_kfree_skb(skb);
 263                                 ret = 0;
 264                         }
 265                         if (l2->down_id == MISDN_ID_NONE) {
 266                                 test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag);
 267                                 mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL);
 268                         }
 269                 }
 270         }
 271         if (!test_and_set_bit(FLG_L1_NOTREADY, &l2->flag)) {
 272                 nskb = skb_dequeue(&l2->down_queue);
 273                 if (nskb) {
 274                         l2->down_id = mISDN_HEAD_ID(nskb);
 275                         if (l2down_skb(l2, nskb)) {
 276                                 dev_kfree_skb(nskb);
 277                                 l2->down_id = MISDN_ID_NONE;
 278                                 test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag);
 279                         }
 280                 } else
 281                         test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag);
 282         }
 283         return ret;
 284 }
 285
 286 static void
 287 l2_timeout(struct FsmInst *fi, int event, void *arg)
 288 {
 289         struct layer2 *l2 = fi->userdata;
 290         struct sk_buff *skb;
 291         struct mISDNhead *hh;
 292
 293         skb = mI_alloc_skb(0, GFP_ATOMIC);
 294         if (!skb) {
 295                 printk(KERN_WARNING "%s: L2(%d,%d) nr:%x timer %s no skb\n",
 296                        mISDNDevName4ch(&l2->ch), l2->sapi, l2->tei,
 297                        l2->ch.nr, event == EV_L2_T200 ? "T200" : "T203");
 298                 return;
 299         }
 300         hh = mISDN_HEAD_P(skb);
 301         hh->prim = event == EV_L2_T200 ? DL_TIMER200_IND : DL_TIMER203_IND;
 302         hh->id = l2->ch.nr;
 303         if (*debug & DEBUG_TIMER)
 304                 printk(KERN_DEBUG "%s: L2(%d,%d) nr:%x timer %s expired\n",
 305                        mISDNDevName4ch(&l2->ch), l2->sapi, l2->tei,
 306                        l2->ch.nr, event == EV_L2_T200 ? "T200" : "T203");
 307         if (l2->ch.st)
 308                 l2->ch.st->own.recv(&l2->ch.st->own, skb);
 309 }
 310
 311 static int
 312 l2mgr(struct layer2 *l2, u_int prim, void *arg) {
 313         long c = (long)arg;
 314
 315         printk(KERN_WARNING "l2mgr: dev %s addr:%x prim %x %c\n",
 316                mISDNDevName4ch(&l2->ch), l2->id, prim, (char)c);
 317         if (test_bit(FLG_LAPD, &l2->flag) &&
 318             !test_bit(FLG_FIXED_TEI, &l2->flag)) {
 319                 switch (c) {
 320                 case 'C':
 321                 case 'D':
 322                 case 'G':
 323                 case 'H':
 324                         l2_tei(l2, prim, (u_long)arg);
 325                         break;
 326                 }
 327         }
 328         return 0;
 329 }
 330
 331 static void
 332 set_peer_busy(struct layer2 *l2) {
 333         test_and_set_bit(FLG_PEER_BUSY, &l2->flag);
 334         if (skb_queue_len(&l2->i_queue) || skb_queue_len(&l2->ui_queue))
 335                 test_and_set_bit(FLG_L2BLOCK, &l2->flag);
 336 }
 337
 338 static void
 339 clear_peer_busy(struct layer2 *l2) {
 340         if (test_and_clear_bit(FLG_PEER_BUSY, &l2->flag))
 341                 test_and_clear_bit(FLG_L2BLOCK, &l2->flag);
 342 }
 343
 344 static void
 345 InitWin(struct layer2 *l2)
 346 {
 347         int i;
 348
 349         for (i = 0; i < MAX_WINDOW; i++)
 350                 l2->windowar[i] = NULL;
 351 }
 352
 353 static int
 354 freewin(struct layer2 *l2)
 355 {
 356         int i, cnt = 0;
 357
 358         for (i = 0; i < MAX_WINDOW; i++) {
 359                 if (l2->windowar[i]) {
 360                         cnt++;
 361                         dev_kfree_skb(l2->windowar[i]);
 362                         l2->windowar[i] = NULL;
 363                 }
 364         }
 365         return cnt;
 366 }
 367
 368 static void
 369 ReleaseWin(struct layer2 *l2)
 370 {
 371         int cnt = freewin(l2);
 372
 373         if (cnt)
 374                 printk(KERN_WARNING
 375                        "isdnl2 freed %d skbuffs in release\n", cnt);
 376 }
 377
 378 inline unsigned int
 379 cansend(struct layer2 *l2)
 380 {
 381         unsigned int p1;
 382
 383         if (test_bit(FLG_MOD128, &l2->flag))
 384                 p1 = (l2->vs - l2->va) % 128;
 385         else
 386                 p1 = (l2->vs - l2->va) % 8;
 387         return (p1 < l2->window) && !test_bit(FLG_PEER_BUSY, &l2->flag);
 388 }
 389
 390 inline void
 391 clear_exception(struct layer2 *l2)
 392 {
 393         test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
 394         test_and_clear_bit(FLG_REJEXC, &l2->flag);
 395         test_and_clear_bit(FLG_OWN_BUSY, &l2->flag);
 396         clear_peer_busy(l2);
 397 }
 398
 399 static int
 400 sethdraddr(struct layer2 *l2, u_char *header, int rsp)
 401 {
 402         u_char *ptr = header;
 403         int crbit = rsp;
 404
 405         if (test_bit(FLG_LAPD, &l2->flag)) {
 406                 if (test_bit(FLG_LAPD_NET, &l2->flag))
 407                         crbit = !crbit;
 408                 *ptr++ = (l2->sapi << 2) | (crbit ? 2 : 0);
 409                 *ptr++ = (l2->tei << 1) | 1;
 410                 return 2;
 411         } else {
 412                 if (test_bit(FLG_ORIG, &l2->flag))
 413                         crbit = !crbit;
 414                 if (crbit)
 415                         *ptr++ = l2->addr.B;
 416                 else
 417                         *ptr++ = l2->addr.A;
 418                 return 1;
 419         }
 420 }
 421
 422 static inline void
 423 enqueue_super(struct layer2 *l2, struct sk_buff *skb)
 424 {
 425         if (l2down(l2, PH_DATA_REQ, l2_newid(l2), skb))
 426                 dev_kfree_skb(skb);
 427 }
 428
 429 static inline void
 430 enqueue_ui(struct layer2 *l2, struct sk_buff *skb)
 431 {
 432         if (l2->tm)
 433                 l2_tei(l2, MDL_STATUS_UI_IND, 0);
 434         if (l2down(l2, PH_DATA_REQ, l2_newid(l2), skb))
 435                 dev_kfree_skb(skb);
 436 }
 437
 438 inline int
 439 IsUI(u_char *data)
 440 {
 441         return (data[0] & 0xef) == UI;
 442 }
 443
 444 inline int
 445 IsUA(u_char *data)
 446 {
 447         return (data[0] & 0xef) == UA;
 448 }
 449
 450 inline int
 451 IsDM(u_char *data)
 452 {
 453         return (data[0] & 0xef) == DM;
 454 }
 455
 456 inline int
 457 IsDISC(u_char *data)
 458 {
 459         return (data[0] & 0xef) == DISC;
 460 }
 461
 462 inline int
 463 IsRR(u_char *data, struct layer2 *l2)
 464 {
 465         if (test_bit(FLG_MOD128, &l2->flag))
 466                 return data[0] == RR;
 467         else
 468                 return (data[0] & 0xf) == 1;
 469 }
 470
 471 inline int
 472 IsSFrame(u_char *data, struct layer2 *l2)
 473 {
 474         register u_char d = *data;
 475
 476         if (!test_bit(FLG_MOD128, &l2->flag))
 477                 d &= 0xf;
 478         return ((d & 0xf3) == 1) && ((d & 0x0c) != 0x0c);
 479 }
 480
 481 inline int
 482 IsSABME(u_char *data, struct layer2 *l2)
 483 {
 484         u_char d = data[0] & ~0x10;
 485
 486         return test_bit(FLG_MOD128, &l2->flag) ? d == SABME : d == SABM;
 487 }
 488
 489 inline int
 490 IsREJ(u_char *data, struct layer2 *l2)
 491 {
 492         return test_bit(FLG_MOD128, &l2->flag) ?
 493                 data[0] == REJ : (data[0] & 0xf) == REJ;
 494 }
 495
 496 inline int
 497 IsFRMR(u_char *data)
 498 {
 499         return (data[0] & 0xef) == FRMR;
 500 }
 501
 502 inline int
 503 IsRNR(u_char *data, struct layer2 *l2)
 504 {
 505         return test_bit(FLG_MOD128, &l2->flag) ?
 506                 data[0] == RNR : (data[0] & 0xf) == RNR;
 507 }
 508
 509 static int
 510 iframe_error(struct layer2 *l2, struct sk_buff *skb)
 511 {
 512         u_int   i;
 513         int     rsp = *skb->data & 0x2;
 514
 515         i = l2addrsize(l2) + (test_bit(FLG_MOD128, &l2->flag) ? 2 : 1);
 516         if (test_bit(FLG_ORIG, &l2->flag))
 517                 rsp = !rsp;
 518         if (rsp)
 519                 return 'L';
 520         if (skb->len < i)
 521                 return 'N';
 522         if ((skb->len - i) > l2->maxlen)
 523                 return 'O';
 524         return 0;
 525 }
 526
 527 static int
 528 super_error(struct layer2 *l2, struct sk_buff *skb)
 529 {
 530         if (skb->len != l2addrsize(l2) +
 531             (test_bit(FLG_MOD128, &l2->flag) ? 2 : 1))
 532                 return 'N';
 533         return 0;
 534 }
 535
 536 static int
 537 unnum_error(struct layer2 *l2, struct sk_buff *skb, int wantrsp)
 538 {
 539         int rsp = (*skb->data & 0x2) >> 1;
 540         if (test_bit(FLG_ORIG, &l2->flag))
 541                 rsp = !rsp;
 542         if (rsp != wantrsp)
 543                 return 'L';
 544         if (skb->len != l2addrsize(l2) + 1)
 545                 return 'N';
 546         return 0;
 547 }
 548
 549 static int
 550 UI_error(struct layer2 *l2, struct sk_buff *skb)
 551 {
 552         int rsp = *skb->data & 0x2;
 553         if (test_bit(FLG_ORIG, &l2->flag))
 554                 rsp = !rsp;
 555         if (rsp)
 556                 return 'L';
 557         if (skb->len > l2->maxlen + l2addrsize(l2) + 1)
 558                 return 'O';
 559         return 0;
 560 }
 561
 562 static int
 563 FRMR_error(struct layer2 *l2, struct sk_buff *skb)
 564 {
 565         u_int   headers = l2addrsize(l2) + 1;
 566         u_char  *datap = skb->data + headers;
 567         int     rsp = *skb->data & 0x2;
 568
 569         if (test_bit(FLG_ORIG, &l2->flag))
 570                 rsp = !rsp;
 571         if (!rsp)
 572                 return 'L';
 573         if (test_bit(FLG_MOD128, &l2->flag)) {
 574                 if (skb->len < headers + 5)
 575                         return 'N';
 576                 else if (*debug & DEBUG_L2)
 577                         l2m_debug(&l2->l2m,
 578                                   "FRMR information %2x %2x %2x %2x %2x",
 579                                   datap[0], datap[1], datap[2], datap[3], datap[4]);
 580         } else {
 581                 if (skb->len < headers + 3)
 582                         return 'N';
 583                 else if (*debug & DEBUG_L2)
 584                         l2m_debug(&l2->l2m,
 585                                   "FRMR information %2x %2x %2x",
 586                                   datap[0], datap[1], datap[2]);
 587         }
 588         return 0;
 589 }
 590
 591 static unsigned int
 592 legalnr(struct layer2 *l2, unsigned int nr)
 593 {
 594         if (test_bit(FLG_MOD128, &l2->flag))
 595                 return ((nr - l2->va) % 128) <= ((l2->vs - l2->va) % 128);
 596         else
 597                 return ((nr - l2->va) % 8) <= ((l2->vs - l2->va) % 8);
 598 }
 599
 600 static void
 601 setva(struct layer2 *l2, unsigned int nr)
 602 {
 603         struct sk_buff  *skb;
 604
 605         while (l2->va != nr) {
 606                 l2->va++;
 607                 if (test_bit(FLG_MOD128, &l2->flag))
 608                         l2->va %= 128;
 609                 else
 610                         l2->va %= 8;
 611                 if (l2->windowar[l2->sow]) {
 612                         skb_trim(l2->windowar[l2->sow], 0);
 613                         skb_queue_tail(&l2->tmp_queue, l2->windowar[l2->sow]);
 614                         l2->windowar[l2->sow] = NULL;
 615                 }
 616                 l2->sow = (l2->sow + 1) % l2->window;
 617         }
 618         skb = skb_dequeue(&l2->tmp_queue);
 619         while (skb) {
 620                 dev_kfree_skb(skb);
 621                 skb = skb_dequeue(&l2->tmp_queue);
 622         }
 623 }
 624
 625 static void
 626 send_uframe(struct layer2 *l2, struct sk_buff *skb, u_char cmd, u_char cr)
 627 {
 628         u_char tmp[MAX_L2HEADER_LEN];
 629         int i;
 630
 631         i = sethdraddr(l2, tmp, cr);
 632         tmp[i++] = cmd;
 633         if (skb)
 634                 skb_trim(skb, 0);
 635         else {
 636                 skb = mI_alloc_skb(i, GFP_ATOMIC);
 637                 if (!skb) {
 638                         printk(KERN_WARNING "%s: can't alloc skbuff in %s\n",
 639                                mISDNDevName4ch(&l2->ch), __func__);
 640                         return;
 641                 }
 642         }
 643         memcpy(skb_put(skb, i), tmp, i);
 644         enqueue_super(l2, skb);
 645 }
 646
 647
 648 inline u_char
 649 get_PollFlag(struct layer2 *l2, struct sk_buff *skb)
 650 {
 651         return skb->data[l2addrsize(l2)] & 0x10;
 652 }
 653
 654 inline u_char
 655 get_PollFlagFree(struct layer2 *l2, struct sk_buff *skb)
 656 {
 657         u_char PF;
 658
 659         PF = get_PollFlag(l2, skb);
 660         dev_kfree_skb(skb);
 661         return PF;
 662 }
 663
 664 inline void
 665 start_t200(struct layer2 *l2, int i)
 666 {
 667         mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, i);
 668         test_and_set_bit(FLG_T200_RUN, &l2->flag);
 669 }
 670
 671 inline void
 672 restart_t200(struct layer2 *l2, int i)
 673 {
 674         mISDN_FsmRestartTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, i);
 675         test_and_set_bit(FLG_T200_RUN, &l2->flag);
 676 }
 677
 678 inline void
 679 stop_t200(struct layer2 *l2, int i)
 680 {
 681         if (test_and_clear_bit(FLG_T200_RUN, &l2->flag))
 682                 mISDN_FsmDelTimer(&l2->t200, i);
 683 }
 684
 685 inline void
 686 st5_dl_release_l2l3(struct layer2 *l2)
 687 {
 688         int pr;
 689
 690         if (test_and_clear_bit(FLG_PEND_REL, &l2->flag))
 691                 pr = DL_RELEASE_CNF;
 692         else
 693                 pr = DL_RELEASE_IND;
 694         l2up_create(l2, pr, 0, NULL);
 695 }
 696
 697 inline void
 698 lapb_dl_release_l2l3(struct layer2 *l2, int f)
 699 {
 700         if (test_bit(FLG_LAPB, &l2->flag))
 701                 l2down_create(l2, PH_DEACTIVATE_REQ, l2_newid(l2), 0, NULL);
 702         l2up_create(l2, f, 0, NULL);
 703 }
 704
 705 static void
 706 establishlink(struct FsmInst *fi)
 707 {
 708         struct layer2 *l2 = fi->userdata;
 709         u_char cmd;
 710
 711         clear_exception(l2);
 712         l2->rc = 0;
 713         cmd = (test_bit(FLG_MOD128, &l2->flag) ? SABME : SABM) | 0x10;
 714         send_uframe(l2, NULL, cmd, CMD);
 715         mISDN_FsmDelTimer(&l2->t203, 1);
 716         restart_t200(l2, 1);
 717         test_and_clear_bit(FLG_PEND_REL, &l2->flag);
 718         freewin(l2);
 719         mISDN_FsmChangeState(fi, ST_L2_5);
 720 }
 721
 722 static void
 723 l2_mdl_error_ua(struct FsmInst *fi, int event, void *arg)
 724 {
 725         struct sk_buff *skb = arg;
 726         struct layer2 *l2 = fi->userdata;
 727
 728         if (get_PollFlagFree(l2, skb))
 729                 l2mgr(l2, MDL_ERROR_IND, (void *) 'C');
 730         else
 731                 l2mgr(l2, MDL_ERROR_IND, (void *) 'D');
 732
 733 }
 734
 735 static void
 736 l2_mdl_error_dm(struct FsmInst *fi, int event, void *arg)
 737 {
 738         struct sk_buff *skb = arg;
 739         struct layer2 *l2 = fi->userdata;
 740
 741         if (get_PollFlagFree(l2, skb))
 742                 l2mgr(l2, MDL_ERROR_IND, (void *) 'B');
 743         else {
 744                 l2mgr(l2, MDL_ERROR_IND, (void *) 'E');
 745                 establishlink(fi);
 746                 test_and_clear_bit(FLG_L3_INIT, &l2->flag);
 747         }
 748 }
 749
 750 static void
 751 l2_st8_mdl_error_dm(struct FsmInst *fi, int event, void *arg)
 752 {
 753         struct sk_buff *skb = arg;
 754         struct layer2 *l2 = fi->userdata;
 755
 756         if (get_PollFlagFree(l2, skb))
 757                 l2mgr(l2, MDL_ERROR_IND, (void *) 'B');
 758         else
 759                 l2mgr(l2, MDL_ERROR_IND, (void *) 'E');
 760         establishlink(fi);
 761         test_and_clear_bit(FLG_L3_INIT, &l2->flag);
 762 }
 763
 764 static void
 765 l2_go_st3(struct FsmInst *fi, int event, void *arg)
 766 {
 767         dev_kfree_skb((struct sk_buff *)arg);
 768         mISDN_FsmChangeState(fi, ST_L2_3);
 769 }
 770
 771 static void
 772 l2_mdl_assign(struct FsmInst *fi, int event, void *arg)
 773 {
 774         struct layer2   *l2 = fi->userdata;
 775
 776         mISDN_FsmChangeState(fi, ST_L2_3);
 777         dev_kfree_skb((struct sk_buff *)arg);
 778         l2_tei(l2, MDL_ASSIGN_IND, 0);
 779 }
 780
 781 static void
 782 l2_queue_ui_assign(struct FsmInst *fi, int event, void *arg)
 783 {
 784         struct layer2 *l2 = fi->userdata;
 785         struct sk_buff *skb = arg;
 786
 787         skb_queue_tail(&l2->ui_queue, skb);
 788         mISDN_FsmChangeState(fi, ST_L2_2);
 789         l2_tei(l2, MDL_ASSIGN_IND, 0);
 790 }
 791
 792 static void
 793 l2_queue_ui(struct FsmInst *fi, int event, void *arg)
 794 {
 795         struct layer2 *l2 = fi->userdata;
 796         struct sk_buff *skb = arg;
 797
 798         skb_queue_tail(&l2->ui_queue, skb);
 799 }
 800
 801 static void
 802 tx_ui(struct layer2 *l2)
 803 {
 804         struct sk_buff *skb;
 805         u_char header[MAX_L2HEADER_LEN];
 806         int i;
 807
 808         i = sethdraddr(l2, header, CMD);
 809         if (test_bit(FLG_LAPD_NET, &l2->flag))
 810                 header[1] = 0xff; /* tei 127 */
 811         header[i++] = UI;
 812         while ((skb = skb_dequeue(&l2->ui_queue))) {
 813                 memcpy(skb_push(skb, i), header, i);
 814                 enqueue_ui(l2, skb);
 815         }
 816 }
 817
 818 static void
 819 l2_send_ui(struct FsmInst *fi, int event, void *arg)
 820 {
 821         struct layer2 *l2 = fi->userdata;
 822         struct sk_buff *skb = arg;
 823
 824         skb_queue_tail(&l2->ui_queue, skb);
 825         tx_ui(l2);
 826 }
 827
 828 static void
 829 l2_got_ui(struct FsmInst *fi, int event, void *arg)
 830 {
 831         struct layer2 *l2 = fi->userdata;
 832         struct sk_buff *skb = arg;
 833
 834         skb_pull(skb, l2headersize(l2, 1));
 835 /*
 836  *              in states 1-3 for broadcast
 837  */
 838
 839         if (l2->tm)
 840                 l2_tei(l2, MDL_STATUS_UI_IND, 0);
 841         l2up(l2, DL_UNITDATA_IND, skb);
 842 }
 843
 844 static void
 845 l2_establish(struct FsmInst *fi, int event, void *arg)
 846 {
 847         struct sk_buff *skb = arg;
 848         struct layer2 *l2 = fi->userdata;
 849
 850         establishlink(fi);
 851         test_and_set_bit(FLG_L3_INIT, &l2->flag);
 852         dev_kfree_skb(skb);
 853 }
 854
 855 static void
 856 l2_discard_i_setl3(struct FsmInst *fi, int event, void *arg)
 857 {
 858         struct sk_buff *skb = arg;
 859         struct layer2 *l2 = fi->userdata;
 860
 861         skb_queue_purge(&l2->i_queue);
 862         test_and_set_bit(FLG_L3_INIT, &l2->flag);
 863         test_and_clear_bit(FLG_PEND_REL, &l2->flag);
 864         dev_kfree_skb(skb);
 865 }
 866
 867 static void
 868 l2_l3_reestablish(struct FsmInst *fi, int event, void *arg)
 869 {
 870         struct sk_buff *skb = arg;
 871         struct layer2 *l2 = fi->userdata;
 872
 873         skb_queue_purge(&l2->i_queue);
 874         establishlink(fi);
 875         test_and_set_bit(FLG_L3_INIT, &l2->flag);
 876         dev_kfree_skb(skb);
 877 }
 878
 879 static void
 880 l2_release(struct FsmInst *fi, int event, void *arg)
 881 {
 882         struct layer2 *l2 = fi->userdata;
 883         struct sk_buff *skb = arg;
 884
 885         skb_trim(skb, 0);
 886         l2up(l2, DL_RELEASE_CNF, skb);
 887 }
 888
 889 static void
 890 l2_pend_rel(struct FsmInst *fi, int event, void *arg)
 891 {
 892         struct sk_buff *skb = arg;
 893         struct layer2 *l2 = fi->userdata;
 894
 895         test_and_set_bit(FLG_PEND_REL, &l2->flag);
 896         dev_kfree_skb(skb);
 897 }
 898
 899 static void
 900 l2_disconnect(struct FsmInst *fi, int event, void *arg)
 901 {
 902         struct layer2 *l2 = fi->userdata;
 903         struct sk_buff *skb = arg;
 904
 905         skb_queue_purge(&l2->i_queue);
 906         freewin(l2);
 907         mISDN_FsmChangeState(fi, ST_L2_6);
 908         l2->rc = 0;
 909         send_uframe(l2, NULL, DISC | 0x10, CMD);
 910         mISDN_FsmDelTimer(&l2->t203, 1);
 911         restart_t200(l2, 2);
 912         if (skb)
 913                 dev_kfree_skb(skb);
 914 }
 915
 916 static void
 917 l2_start_multi(struct FsmInst *fi, int event, void *arg)
 918 {
 919         struct layer2   *l2 = fi->userdata;
 920         struct sk_buff  *skb = arg;
 921
 922         l2->vs = 0;
 923         l2->va = 0;
 924         l2->vr = 0;
 925         l2->sow = 0;
 926         clear_exception(l2);
 927         send_uframe(l2, NULL, UA | get_PollFlag(l2, skb), RSP);
 928         mISDN_FsmChangeState(fi, ST_L2_7);
 929         mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 3);
 930         skb_trim(skb, 0);
 931         l2up(l2, DL_ESTABLISH_IND, skb);
 932         if (l2->tm)
 933                 l2_tei(l2, MDL_STATUS_UP_IND, 0);
 934 }
 935
 936 static void
 937 l2_send_UA(struct FsmInst *fi, int event, void *arg)
 938 {
 939         struct layer2 *l2 = fi->userdata;
 940         struct sk_buff *skb = arg;
 941
 942         send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP);
 943 }
 944
 945 static void
 946 l2_send_DM(struct FsmInst *fi, int event, void *arg)
 947 {
 948         struct layer2 *l2 = fi->userdata;
 949         struct sk_buff *skb = arg;
 950
 951         send_uframe(l2, skb, DM | get_PollFlag(l2, skb), RSP);
 952 }
 953
 954 static void
 955 l2_restart_multi(struct FsmInst *fi, int event, void *arg)
 956 {
 957         struct layer2   *l2 = fi->userdata;
 958         struct sk_buff  *skb = arg;
 959         int             est = 0;
 960
 961         send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP);
 962
 963         l2mgr(l2, MDL_ERROR_IND, (void *) 'F');
 964
 965         if (l2->vs != l2->va) {
 966                 skb_queue_purge(&l2->i_queue);
 967                 est = 1;
 968         }
 969
 970         clear_exception(l2);
 971         l2->vs = 0;
 972         l2->va = 0;
 973         l2->vr = 0;
 974         l2->sow = 0;
 975         mISDN_FsmChangeState(fi, ST_L2_7);
 976         stop_t200(l2, 3);
 977         mISDN_FsmRestartTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 3);
 978
 979         if (est)
 980                 l2up_create(l2, DL_ESTABLISH_IND, 0, NULL);
 981 /*              mISDN_queue_data(&l2->inst, l2->inst.id | MSG_BROADCAST,
 982  *                  MGR_SHORTSTATUS | INDICATION, SSTATUS_L2_ESTABLISHED,
 983  *                  0, NULL, 0);
 984  */
 985         if (skb_queue_len(&l2->i_queue) && cansend(l2))
 986                 mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
 987 }
 988
 989 static void
 990 l2_stop_multi(struct FsmInst *fi, int event, void *arg)
 991 {
 992         struct layer2   *l2 = fi->userdata;
 993         struct sk_buff  *skb = arg;
 994
 995         mISDN_FsmChangeState(fi, ST_L2_4);
 996         mISDN_FsmDelTimer(&l2->t203, 3);
 997         stop_t200(l2, 4);
 998
 999         send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP);
1000         skb_queue_purge(&l2->i_queue);
1001         freewin(l2);
1002         lapb_dl_release_l2l3(l2, DL_RELEASE_IND);
1003         if (l2->tm)
1004                 l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1005 }
1006
1007 static void
1008 l2_connected(struct FsmInst *fi, int event, void *arg)
1009 {
1010         struct layer2   *l2 = fi->userdata;
1011         struct sk_buff  *skb = arg;
1012         int pr = -1;
1013
1014         if (!get_PollFlag(l2, skb)) {
1015                 l2_mdl_error_ua(fi, event, arg);
1016                 return;
1017         }
1018         dev_kfree_skb(skb);
1019         if (test_and_clear_bit(FLG_PEND_REL, &l2->flag))
1020                 l2_disconnect(fi, event, NULL);
1021         if (test_and_clear_bit(FLG_L3_INIT, &l2->flag)) {
1022                 pr = DL_ESTABLISH_CNF;
1023         } else if (l2->vs != l2->va) {
1024                 skb_queue_purge(&l2->i_queue);
1025                 pr = DL_ESTABLISH_IND;
1026         }
1027         stop_t200(l2, 5);
1028         l2->vr = 0;
1029         l2->vs = 0;
1030         l2->va = 0;
1031         l2->sow = 0;
1032         mISDN_FsmChangeState(fi, ST_L2_7);
1033         mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 4);
1034         if (pr != -1)
1035                 l2up_create(l2, pr, 0, NULL);
1036
1037         if (skb_queue_len(&l2->i_queue) && cansend(l2))
1038                 mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
1039
1040         if (l2->tm)
1041                 l2_tei(l2, MDL_STATUS_UP_IND, 0);
1042 }
1043
1044 static void
1045 l2_released(struct FsmInst *fi, int event, void *arg)
1046 {
1047         struct layer2 *l2 = fi->userdata;
1048         struct sk_buff *skb = arg;
1049
1050         if (!get_PollFlag(l2, skb)) {
1051                 l2_mdl_error_ua(fi, event, arg);
1052                 return;
1053         }
1054         dev_kfree_skb(skb);
1055         stop_t200(l2, 6);
1056         lapb_dl_release_l2l3(l2, DL_RELEASE_CNF);
1057         mISDN_FsmChangeState(fi, ST_L2_4);
1058         if (l2->tm)
1059                 l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1060 }
1061
1062 static void
1063 l2_reestablish(struct FsmInst *fi, int event, void *arg)
1064 {
1065         struct layer2 *l2 = fi->userdata;
1066         struct sk_buff *skb = arg;
1067
1068         if (!get_PollFlagFree(l2, skb)) {
1069                 establishlink(fi);
1070                 test_and_set_bit(FLG_L3_INIT, &l2->flag);
1071         }
1072 }
1073
1074 static void
1075 l2_st5_dm_release(struct FsmInst *fi, int event, void *arg)
1076 {
1077         struct layer2 *l2 = fi->userdata;
1078         struct sk_buff *skb = arg;
1079
1080         if (get_PollFlagFree(l2, skb)) {
1081                 stop_t200(l2, 7);
1082                 if (!test_bit(FLG_L3_INIT, &l2->flag))
1083                         skb_queue_purge(&l2->i_queue);
1084                 if (test_bit(FLG_LAPB, &l2->flag))
1085                         l2down_create(l2, PH_DEACTIVATE_REQ,
1086                                       l2_newid(l2), 0, NULL);
1087                 st5_dl_release_l2l3(l2);
1088                 mISDN_FsmChangeState(fi, ST_L2_4);
1089                 if (l2->tm)
1090                         l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1091         }
1092 }
1093
1094 static void
1095 l2_st6_dm_release(struct FsmInst *fi, int event, void *arg)
1096 {
1097         struct layer2 *l2 = fi->userdata;
1098         struct sk_buff *skb = arg;
1099
1100         if (get_PollFlagFree(l2, skb)) {
1101                 stop_t200(l2, 8);
1102                 lapb_dl_release_l2l3(l2, DL_RELEASE_CNF);
1103                 mISDN_FsmChangeState(fi, ST_L2_4);
1104                 if (l2->tm)
1105                         l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1106         }
1107 }
1108
1109 static void
1110 enquiry_cr(struct layer2 *l2, u_char typ, u_char cr, u_char pf)
1111 {
1112         struct sk_buff *skb;
1113         u_char tmp[MAX_L2HEADER_LEN];
1114         int i;
1115
1116         i = sethdraddr(l2, tmp, cr);
1117         if (test_bit(FLG_MOD128, &l2->flag)) {
1118                 tmp[i++] = typ;
1119                 tmp[i++] = (l2->vr << 1) | (pf ? 1 : 0);
1120         } else
1121                 tmp[i++] = (l2->vr << 5) | typ | (pf ? 0x10 : 0);
1122         skb = mI_alloc_skb(i, GFP_ATOMIC);
1123         if (!skb) {
1124                 printk(KERN_WARNING "%s: isdnl2 can't alloc sbbuff in %s\n",
1125                        mISDNDevName4ch(&l2->ch), __func__);
1126                 return;
1127         }
1128         memcpy(skb_put(skb, i), tmp, i);
1129         enqueue_super(l2, skb);
1130 }
1131
1132 inline void
1133 enquiry_response(struct layer2 *l2)
1134 {
1135         if (test_bit(FLG_OWN_BUSY, &l2->flag))
1136                 enquiry_cr(l2, RNR, RSP, 1);
1137         else
1138                 enquiry_cr(l2, RR, RSP, 1);
1139         test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1140 }
1141
1142 inline void
1143 transmit_enquiry(struct layer2 *l2)
1144 {
1145         if (test_bit(FLG_OWN_BUSY, &l2->flag))
1146                 enquiry_cr(l2, RNR, CMD, 1);
1147         else
1148                 enquiry_cr(l2, RR, CMD, 1);
1149         test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1150         start_t200(l2, 9);
1151 }
1152
1153
1154 static void
1155 nrerrorrecovery(struct FsmInst *fi)
1156 {
1157         struct layer2 *l2 = fi->userdata;
1158
1159         l2mgr(l2, MDL_ERROR_IND, (void *) 'J');
1160         establishlink(fi);
1161         test_and_clear_bit(FLG_L3_INIT, &l2->flag);
1162 }
1163
1164 static void
1165 invoke_retransmission(struct layer2 *l2, unsigned int nr)
1166 {
1167         u_int   p1;
1168
1169         if (l2->vs != nr) {
1170                 while (l2->vs != nr) {
1171                         (l2->vs)--;
1172                         if (test_bit(FLG_MOD128, &l2->flag)) {
1173                                 l2->vs %= 128;
1174                                 p1 = (l2->vs - l2->va) % 128;
1175                         } else {
1176                                 l2->vs %= 8;
1177                                 p1 = (l2->vs - l2->va) % 8;
1178                         }
1179                         p1 = (p1 + l2->sow) % l2->window;
1180                         if (l2->windowar[p1])
1181                                 skb_queue_head(&l2->i_queue, l2->windowar[p1]);
1182                         else
1183                                 printk(KERN_WARNING
1184                                        "%s: windowar[%d] is NULL\n",
1185                                        mISDNDevName4ch(&l2->ch), p1);
1186                         l2->windowar[p1] = NULL;
1187                 }
1188                 mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL);
1189         }
1190 }
1191
1192 static void
1193 l2_st7_got_super(struct FsmInst *fi, int event, void *arg)
1194 {
1195         struct layer2 *l2 = fi->userdata;
1196         struct sk_buff *skb = arg;
1197         int PollFlag, rsp, typ = RR;
1198         unsigned int nr;
1199
1200         rsp = *skb->data & 0x2;
1201         if (test_bit(FLG_ORIG, &l2->flag))
1202                 rsp = !rsp;
1203
1204         skb_pull(skb, l2addrsize(l2));
1205         if (IsRNR(skb->data, l2)) {
1206                 set_peer_busy(l2);
1207                 typ = RNR;
1208         } else
1209                 clear_peer_busy(l2);
1210         if (IsREJ(skb->data, l2))
1211                 typ = REJ;
1212
1213         if (test_bit(FLG_MOD128, &l2->flag)) {
1214                 PollFlag = (skb->data[1] & 0x1) == 0x1;
1215                 nr = skb->data[1] >> 1;
1216         } else {
1217                 PollFlag = (skb->data[0] & 0x10);
1218                 nr = (skb->data[0] >> 5) & 0x7;
1219         }
1220         dev_kfree_skb(skb);
1221
1222         if (PollFlag) {
1223                 if (rsp)
1224                         l2mgr(l2, MDL_ERROR_IND, (void *) 'A');
1225                 else
1226                         enquiry_response(l2);
1227         }
1228         if (legalnr(l2, nr)) {
1229                 if (typ == REJ) {
1230                         setva(l2, nr);
1231                         invoke_retransmission(l2, nr);
1232                         stop_t200(l2, 10);
1233                         if (mISDN_FsmAddTimer(&l2->t203, l2->T203,
1234                                               EV_L2_T203, NULL, 6))
1235                                 l2m_debug(&l2->l2m, "Restart T203 ST7 REJ");
1236                 } else if ((nr == l2->vs) && (typ == RR)) {
1237                         setva(l2, nr);
1238                         stop_t200(l2, 11);
1239                         mISDN_FsmRestartTimer(&l2->t203, l2->T203,
1240                                               EV_L2_T203, NULL, 7);
1241                 } else if ((l2->va != nr) || (typ == RNR)) {
1242                         setva(l2, nr);
1243                         if (typ != RR)
1244                                 mISDN_FsmDelTimer(&l2->t203, 9);
1245                         restart_t200(l2, 12);
1246                 }
1247                 if (skb_queue_len(&l2->i_queue) && (typ == RR))
1248                         mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
1249         } else
1250                 nrerrorrecovery(fi);
1251 }
1252
1253 static void
1254 l2_feed_i_if_reest(struct FsmInst *fi, int event, void *arg)
1255 {
1256         struct layer2 *l2 = fi->userdata;
1257         struct sk_buff *skb = arg;
1258
1259         if (!test_bit(FLG_L3_INIT, &l2->flag))
1260                 skb_queue_tail(&l2->i_queue, skb);
1261         else
1262                 dev_kfree_skb(skb);
1263 }
1264
1265 static void
1266 l2_feed_i_pull(struct FsmInst *fi, int event, void *arg)
1267 {
1268         struct layer2 *l2 = fi->userdata;
1269         struct sk_buff *skb = arg;
1270
1271         skb_queue_tail(&l2->i_queue, skb);
1272         mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
1273 }
1274
1275 static void
1276 l2_feed_iqueue(struct FsmInst *fi, int event, void *arg)
1277 {
1278         struct layer2 *l2 = fi->userdata;
1279         struct sk_buff *skb = arg;
1280
1281         skb_queue_tail(&l2->i_queue, skb);
1282 }
1283
1284 static void
1285 l2_got_iframe(struct FsmInst *fi, int event, void *arg)
1286 {
1287         struct layer2   *l2 = fi->userdata;
1288         struct sk_buff  *skb = arg;
1289         int             PollFlag, i;
1290         u_int           ns, nr;
1291
1292         i = l2addrsize(l2);
1293         if (test_bit(FLG_MOD128, &l2->flag)) {
1294                 PollFlag = ((skb->data[i + 1] & 0x1) == 0x1);
1295                 ns = skb->data[i] >> 1;
1296                 nr = (skb->data[i + 1] >> 1) & 0x7f;
1297         } else {
1298                 PollFlag = (skb->data[i] & 0x10);
1299                 ns = (skb->data[i] >> 1) & 0x7;
1300                 nr = (skb->data[i] >> 5) & 0x7;
1301         }
1302         if (test_bit(FLG_OWN_BUSY, &l2->flag)) {
1303                 dev_kfree_skb(skb);
1304                 if (PollFlag)
1305                         enquiry_response(l2);
1306         } else {
1307                 if (l2->vr == ns) {
1308                         l2->vr++;
1309                         if (test_bit(FLG_MOD128, &l2->flag))
1310                                 l2->vr %= 128;
1311                         else
1312                                 l2->vr %= 8;
1313                         test_and_clear_bit(FLG_REJEXC, &l2->flag);
1314                         if (PollFlag)
1315                                 enquiry_response(l2);
1316                         else
1317                                 test_and_set_bit(FLG_ACK_PEND, &l2->flag);
1318                         skb_pull(skb, l2headersize(l2, 0));
1319                         l2up(l2, DL_DATA_IND, skb);
1320                 } else {
1321                         /* n(s)!=v(r) */
1322                         dev_kfree_skb(skb);
1323                         if (test_and_set_bit(FLG_REJEXC, &l2->flag)) {
1324                                 if (PollFlag)
1325                                         enquiry_response(l2);
1326                         } else {
1327                                 enquiry_cr(l2, REJ, RSP, PollFlag);
1328                                 test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1329                         }
1330                 }
1331         }
1332         if (legalnr(l2, nr)) {
1333                 if (!test_bit(FLG_PEER_BUSY, &l2->flag) &&
1334                     (fi->state == ST_L2_7)) {
1335                         if (nr == l2->vs) {
1336                                 stop_t200(l2, 13);
1337                                 mISDN_FsmRestartTimer(&l2->t203, l2->T203,
1338                                                       EV_L2_T203, NULL, 7);
1339                         } else if (nr != l2->va)
1340                                 restart_t200(l2, 14);
1341                 }
1342                 setva(l2, nr);
1343         } else {
1344                 nrerrorrecovery(fi);
1345                 return;
1346         }
1347         if (skb_queue_len(&l2->i_queue) && (fi->state == ST_L2_7))
1348                 mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
1349         if (test_and_clear_bit(FLG_ACK_PEND, &l2->flag))
1350                 enquiry_cr(l2, RR, RSP, 0);
1351 }
1352
1353 static void
1354 l2_got_tei(struct FsmInst *fi, int event, void *arg)
1355 {
1356         struct layer2   *l2 = fi->userdata;
1357         u_int           info;
1358
1359         l2->tei = (signed char)(long)arg;
1360         set_channel_address(&l2->ch, l2->sapi, l2->tei);
1361         info = DL_INFO_L2_CONNECT;
1362         l2up_create(l2, DL_INFORMATION_IND, sizeof(info), &info);
1363         if (fi->state == ST_L2_3) {
1364                 establishlink(fi);
1365                 test_and_set_bit(FLG_L3_INIT, &l2->flag);
1366         } else
1367                 mISDN_FsmChangeState(fi, ST_L2_4);
1368         if (skb_queue_len(&l2->ui_queue))
1369                 tx_ui(l2);
1370 }
1371
1372 static void
1373 l2_st5_tout_200(struct FsmInst *fi, int event, void *arg)
1374 {
1375         struct layer2 *l2 = fi->userdata;
1376
1377         if (test_bit(FLG_LAPD, &l2->flag) &&
1378             test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
1379                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
1380         } else if (l2->rc == l2->N200) {
1381                 mISDN_FsmChangeState(fi, ST_L2_4);
1382                 test_and_clear_bit(FLG_T200_RUN, &l2->flag);
1383                 skb_queue_purge(&l2->i_queue);
1384                 l2mgr(l2, MDL_ERROR_IND, (void *) 'G');
1385                 if (test_bit(FLG_LAPB, &l2->flag))
1386                         l2down_create(l2, PH_DEACTIVATE_REQ,
1387                                       l2_newid(l2), 0, NULL);
1388                 st5_dl_release_l2l3(l2);
1389                 if (l2->tm)
1390                         l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1391         } else {
1392                 l2->rc++;
1393                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
1394                 send_uframe(l2, NULL, (test_bit(FLG_MOD128, &l2->flag) ?
1395                                        SABME : SABM) | 0x10, CMD);
1396         }
1397 }
1398
1399 static void
1400 l2_st6_tout_200(struct FsmInst *fi, int event, void *arg)
1401 {
1402         struct layer2 *l2 = fi->userdata;
1403
1404         if (test_bit(FLG_LAPD, &l2->flag) &&
1405             test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
1406                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
1407         } else if (l2->rc == l2->N200) {
1408                 mISDN_FsmChangeState(fi, ST_L2_4);
1409                 test_and_clear_bit(FLG_T200_RUN, &l2->flag);
1410                 l2mgr(l2, MDL_ERROR_IND, (void *) 'H');
1411                 lapb_dl_release_l2l3(l2, DL_RELEASE_CNF);
1412                 if (l2->tm)
1413                         l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1414         } else {
1415                 l2->rc++;
1416                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200,
1417                                   NULL, 9);
1418                 send_uframe(l2, NULL, DISC | 0x10, CMD);
1419         }
1420 }
1421
1422 static void
1423 l2_st7_tout_200(struct FsmInst *fi, int event, void *arg)
1424 {
1425         struct layer2 *l2 = fi->userdata;
1426
1427         if (test_bit(FLG_LAPD, &l2->flag) &&
1428             test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
1429                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
1430                 return;
1431         }
1432         test_and_clear_bit(FLG_T200_RUN, &l2->flag);
1433         l2->rc = 0;
1434         mISDN_FsmChangeState(fi, ST_L2_8);
1435         transmit_enquiry(l2);
1436         l2->rc++;
1437 }
1438
1439 static void
1440 l2_st8_tout_200(struct FsmInst *fi, int event, void *arg)
1441 {
1442         struct layer2 *l2 = fi->userdata;
1443
1444         if (test_bit(FLG_LAPD, &l2->flag) &&
1445             test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
1446                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9);
1447                 return;
1448         }
1449         test_and_clear_bit(FLG_T200_RUN, &l2->flag);
1450         if (l2->rc == l2->N200) {
1451                 l2mgr(l2, MDL_ERROR_IND, (void *) 'I');
1452                 establishlink(fi);
1453                 test_and_clear_bit(FLG_L3_INIT, &l2->flag);
1454         } else {
1455                 transmit_enquiry(l2);
1456                 l2->rc++;
1457         }
1458 }
1459
1460 static void
1461 l2_st7_tout_203(struct FsmInst *fi, int event, void *arg)
1462 {
1463         struct layer2 *l2 = fi->userdata;
1464
1465         if (test_bit(FLG_LAPD, &l2->flag) &&
1466             test_bit(FLG_DCHAN_BUSY, &l2->flag)) {
1467                 mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 9);
1468                 return;
1469         }
1470         mISDN_FsmChangeState(fi, ST_L2_8);
1471         transmit_enquiry(l2);
1472         l2->rc = 0;
1473 }
1474
1475 static void
1476 l2_pull_iqueue(struct FsmInst *fi, int event, void *arg)
1477 {
1478         struct layer2   *l2 = fi->userdata;
1479         struct sk_buff  *skb, *nskb;
1480         u_char          header[MAX_L2HEADER_LEN];
1481         u_int           i, p1;
1482
1483         if (!cansend(l2))
1484                 return;
1485
1486         skb = skb_dequeue(&l2->i_queue);
1487         if (!skb)
1488                 return;
1489         i = sethdraddr(l2, header, CMD);
1490         if (test_bit(FLG_MOD128, &l2->flag)) {
1491                 header[i++] = l2->vs << 1;
1492                 header[i++] = l2->vr << 1;
1493         } else
1494                 header[i++] = (l2->vr << 5) | (l2->vs << 1);
1495         nskb = skb_realloc_headroom(skb, i);
1496         if (!nskb) {
1497                 printk(KERN_WARNING "%s: no headroom(%d) copy for IFrame\n",
1498                        mISDNDevName4ch(&l2->ch), i);
1499                 skb_queue_head(&l2->i_queue, skb);
1500                 return;
1501         }
1502         if (test_bit(FLG_MOD128, &l2->flag)) {
1503                 p1 = (l2->vs - l2->va) % 128;
1504                 l2->vs = (l2->vs + 1) % 128;
1505         } else {
1506                 p1 = (l2->vs - l2->va) % 8;
1507                 l2->vs = (l2->vs + 1) % 8;
1508         }
1509         p1 = (p1 + l2->sow) % l2->window;
1510         if (l2->windowar[p1]) {
1511                 printk(KERN_WARNING "%s: l2 try overwrite ack queue entry %d\n",
1512                        mISDNDevName4ch(&l2->ch), p1);
1513                 dev_kfree_skb(l2->windowar[p1]);
1514         }
1515         l2->windowar[p1] = skb;
1516         memcpy(skb_push(nskb, i), header, i);
1517         l2down(l2, PH_DATA_REQ, l2_newid(l2), nskb);
1518         test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1519         if (!test_and_set_bit(FLG_T200_RUN, &l2->flag)) {
1520                 mISDN_FsmDelTimer(&l2->t203, 13);
1521                 mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 11);
1522         }
1523 }
1524
1525 static void
1526 l2_st8_got_super(struct FsmInst *fi, int event, void *arg)
1527 {
1528         struct layer2 *l2 = fi->userdata;
1529         struct sk_buff *skb = arg;
1530         int PollFlag, rsp, rnr = 0;
1531         unsigned int nr;
1532
1533         rsp = *skb->data & 0x2;
1534         if (test_bit(FLG_ORIG, &l2->flag))
1535                 rsp = !rsp;
1536
1537         skb_pull(skb, l2addrsize(l2));
1538
1539         if (IsRNR(skb->data, l2)) {
1540                 set_peer_busy(l2);
1541                 rnr = 1;
1542         } else
1543                 clear_peer_busy(l2);
1544
1545         if (test_bit(FLG_MOD128, &l2->flag)) {
1546                 PollFlag = (skb->data[1] & 0x1) == 0x1;
1547                 nr = skb->data[1] >> 1;
1548         } else {
1549                 PollFlag = (skb->data[0] & 0x10);
1550                 nr = (skb->data[0] >> 5) & 0x7;
1551         }
1552         dev_kfree_skb(skb);
1553         if (rsp && PollFlag) {
1554                 if (legalnr(l2, nr)) {
1555                         if (rnr) {
1556                                 restart_t200(l2, 15);
1557                         } else {
1558                                 stop_t200(l2, 16);
1559                                 mISDN_FsmAddTimer(&l2->t203, l2->T203,
1560                                                   EV_L2_T203, NULL, 5);
1561                                 setva(l2, nr);
1562                         }
1563                         invoke_retransmission(l2, nr);
1564                         mISDN_FsmChangeState(fi, ST_L2_7);
1565                         if (skb_queue_len(&l2->i_queue) && cansend(l2))
1566                                 mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL);
1567                 } else
1568                         nrerrorrecovery(fi);
1569         } else {
1570                 if (!rsp && PollFlag)
1571                         enquiry_response(l2);
1572                 if (legalnr(l2, nr))
1573                         setva(l2, nr);
1574                 else
1575                         nrerrorrecovery(fi);
1576         }
1577 }
1578
1579 static void
1580 l2_got_FRMR(struct FsmInst *fi, int event, void *arg)
1581 {
1582         struct layer2 *l2 = fi->userdata;
1583         struct sk_buff *skb = arg;
1584
1585         skb_pull(skb, l2addrsize(l2) + 1);
1586
1587         if (!(skb->data[0] & 1) || ((skb->data[0] & 3) == 1) || /* I or S */
1588             (IsUA(skb->data) && (fi->state == ST_L2_7))) {
1589                 l2mgr(l2, MDL_ERROR_IND, (void *) 'K');
1590                 establishlink(fi);
1591                 test_and_clear_bit(FLG_L3_INIT, &l2->flag);
1592         }
1593         dev_kfree_skb(skb);
1594 }
1595
1596 static void
1597 l2_st24_tei_remove(struct FsmInst *fi, int event, void *arg)
1598 {
1599         struct layer2 *l2 = fi->userdata;
1600
1601         skb_queue_purge(&l2->ui_queue);
1602         l2->tei = GROUP_TEI;
1603         mISDN_FsmChangeState(fi, ST_L2_1);
1604 }
1605
1606 static void
1607 l2_st3_tei_remove(struct FsmInst *fi, int event, void *arg)
1608 {
1609         struct layer2 *l2 = fi->userdata;
1610
1611         skb_queue_purge(&l2->ui_queue);
1612         l2->tei = GROUP_TEI;
1613         l2up_create(l2, DL_RELEASE_IND, 0, NULL);
1614         mISDN_FsmChangeState(fi, ST_L2_1);
1615 }
1616
1617 static void
1618 l2_st5_tei_remove(struct FsmInst *fi, int event, void *arg)
1619 {
1620         struct layer2 *l2 = fi->userdata;
1621
1622         skb_queue_purge(&l2->i_queue);
1623         skb_queue_purge(&l2->ui_queue);
1624         freewin(l2);
1625         l2->tei = GROUP_TEI;
1626         stop_t200(l2, 17);
1627         st5_dl_release_l2l3(l2);
1628         mISDN_FsmChangeState(fi, ST_L2_1);
1629 }
1630
1631 static void
1632 l2_st6_tei_remove(struct FsmInst *fi, int event, void *arg)
1633 {
1634         struct layer2 *l2 = fi->userdata;
1635
1636         skb_queue_purge(&l2->ui_queue);
1637         l2->tei = GROUP_TEI;
1638         stop_t200(l2, 18);
1639         l2up_create(l2, DL_RELEASE_IND, 0, NULL);
1640         mISDN_FsmChangeState(fi, ST_L2_1);
1641 }
1642
1643 static void
1644 l2_tei_remove(struct FsmInst *fi, int event, void *arg)
1645 {
1646         struct layer2 *l2 = fi->userdata;
1647
1648         skb_queue_purge(&l2->i_queue);
1649         skb_queue_purge(&l2->ui_queue);
1650         freewin(l2);
1651         l2->tei = GROUP_TEI;
1652         stop_t200(l2, 17);
1653         mISDN_FsmDelTimer(&l2->t203, 19);
1654         l2up_create(l2, DL_RELEASE_IND, 0, NULL);
1655 /*      mISDN_queue_data(&l2->inst, l2->inst.id | MSG_BROADCAST,
1656  *              MGR_SHORTSTATUS_IND, SSTATUS_L2_RELEASED,
1657  *              0, NULL, 0);
1658  */
1659         mISDN_FsmChangeState(fi, ST_L2_1);
1660 }
1661
1662 static void
1663 l2_st14_persistent_da(struct FsmInst *fi, int event, void *arg)
1664 {
1665         struct layer2 *l2 = fi->userdata;
1666         struct sk_buff *skb = arg;
1667
1668         skb_queue_purge(&l2->i_queue);
1669         skb_queue_purge(&l2->ui_queue);
1670         if (test_and_clear_bit(FLG_ESTAB_PEND, &l2->flag))
1671                 l2up(l2, DL_RELEASE_IND, skb);
1672         else
1673                 dev_kfree_skb(skb);
1674 }
1675
1676 static void
1677 l2_st5_persistent_da(struct FsmInst *fi, int event, void *arg)
1678 {
1679         struct layer2 *l2 = fi->userdata;
1680         struct sk_buff *skb = arg;
1681
1682         skb_queue_purge(&l2->i_queue);
1683         skb_queue_purge(&l2->ui_queue);
1684         freewin(l2);
1685         stop_t200(l2, 19);
1686         st5_dl_release_l2l3(l2);
1687         mISDN_FsmChangeState(fi, ST_L2_4);
1688         if (l2->tm)
1689                 l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1690         dev_kfree_skb(skb);
1691 }
1692
1693 static void
1694 l2_st6_persistent_da(struct FsmInst *fi, int event, void *arg)
1695 {
1696         struct layer2 *l2 = fi->userdata;
1697         struct sk_buff *skb = arg;
1698
1699         skb_queue_purge(&l2->ui_queue);
1700         stop_t200(l2, 20);
1701         l2up(l2, DL_RELEASE_CNF, skb);
1702         mISDN_FsmChangeState(fi, ST_L2_4);
1703         if (l2->tm)
1704                 l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1705 }
1706
1707 static void
1708 l2_persistent_da(struct FsmInst *fi, int event, void *arg)
1709 {
1710         struct layer2 *l2 = fi->userdata;
1711         struct sk_buff *skb = arg;
1712
1713         skb_queue_purge(&l2->i_queue);
1714         skb_queue_purge(&l2->ui_queue);
1715         freewin(l2);
1716         stop_t200(l2, 19);
1717         mISDN_FsmDelTimer(&l2->t203, 19);
1718         l2up(l2, DL_RELEASE_IND, skb);
1719         mISDN_FsmChangeState(fi, ST_L2_4);
1720         if (l2->tm)
1721                 l2_tei(l2, MDL_STATUS_DOWN_IND, 0);
1722 }
1723
1724 static void
1725 l2_set_own_busy(struct FsmInst *fi, int event, void *arg)
1726 {
1727         struct layer2 *l2 = fi->userdata;
1728         struct sk_buff *skb = arg;
1729
1730         if (!test_and_set_bit(FLG_OWN_BUSY, &l2->flag)) {
1731                 enquiry_cr(l2, RNR, RSP, 0);
1732                 test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1733         }
1734         if (skb)
1735                 dev_kfree_skb(skb);
1736 }
1737
1738 static void
1739 l2_clear_own_busy(struct FsmInst *fi, int event, void *arg)
1740 {
1741         struct layer2 *l2 = fi->userdata;
1742         struct sk_buff *skb = arg;
1743
1744         if (!test_and_clear_bit(FLG_OWN_BUSY, &l2->flag)) {
1745                 enquiry_cr(l2, RR, RSP, 0);
1746                 test_and_clear_bit(FLG_ACK_PEND, &l2->flag);
1747         }
1748         if (skb)
1749                 dev_kfree_skb(skb);
1750 }
1751
1752 static void
1753 l2_frame_error(struct FsmInst *fi, int event, void *arg)
1754 {
1755         struct layer2 *l2 = fi->userdata;
1756
1757         l2mgr(l2, MDL_ERROR_IND, arg);
1758 }
1759
1760 static void
1761 l2_frame_error_reest(struct FsmInst *fi, int event, void *arg)
1762 {
1763         struct layer2 *l2 = fi->userdata;
1764
1765         l2mgr(l2, MDL_ERROR_IND, arg);
1766         establishlink(fi);
1767         test_and_clear_bit(FLG_L3_INIT, &l2->flag);
1768 }
1769
1770 static struct FsmNode L2FnList[] =
1771 {
1772         {ST_L2_1, EV_L2_DL_ESTABLISH_REQ, l2_mdl_assign},
1773         {ST_L2_2, EV_L2_DL_ESTABLISH_REQ, l2_go_st3},
1774         {ST_L2_4, EV_L2_DL_ESTABLISH_REQ, l2_establish},
1775         {ST_L2_5, EV_L2_DL_ESTABLISH_REQ, l2_discard_i_setl3},
1776         {ST_L2_7, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish},
1777         {ST_L2_8, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish},
1778         {ST_L2_4, EV_L2_DL_RELEASE_REQ, l2_release},
1779         {ST_L2_5, EV_L2_DL_RELEASE_REQ, l2_pend_rel},
1780         {ST_L2_7, EV_L2_DL_RELEASE_REQ, l2_disconnect},
1781         {ST_L2_8, EV_L2_DL_RELEASE_REQ, l2_disconnect},
1782         {ST_L2_5, EV_L2_DL_DATA, l2_feed_i_if_reest},
1783         {ST_L2_7, EV_L2_DL_DATA, l2_feed_i_pull},
1784         {ST_L2_8, EV_L2_DL_DATA, l2_feed_iqueue},
1785         {ST_L2_1, EV_L2_DL_UNITDATA, l2_queue_ui_assign},
1786         {ST_L2_2, EV_L2_DL_UNITDATA, l2_queue_ui},
1787         {ST_L2_3, EV_L2_DL_UNITDATA, l2_queue_ui},
1788         {ST_L2_4, EV_L2_DL_UNITDATA, l2_send_ui},
1789         {ST_L2_5, EV_L2_DL_UNITDATA, l2_send_ui},
1790         {ST_L2_6, EV_L2_DL_UNITDATA, l2_send_ui},
1791         {ST_L2_7, EV_L2_DL_UNITDATA, l2_send_ui},
1792         {ST_L2_8, EV_L2_DL_UNITDATA, l2_send_ui},
1793         {ST_L2_1, EV_L2_MDL_ASSIGN, l2_got_tei},
1794         {ST_L2_2, EV_L2_MDL_ASSIGN, l2_got_tei},
1795         {ST_L2_3, EV_L2_MDL_ASSIGN, l2_got_tei},
1796         {ST_L2_2, EV_L2_MDL_ERROR, l2_st24_tei_remove},
1797         {ST_L2_3, EV_L2_MDL_ERROR, l2_st3_tei_remove},
1798         {ST_L2_4, EV_L2_MDL_REMOVE, l2_st24_tei_remove},
1799         {ST_L2_5, EV_L2_MDL_REMOVE, l2_st5_tei_remove},
1800         {ST_L2_6, EV_L2_MDL_REMOVE, l2_st6_tei_remove},
1801         {ST_L2_7, EV_L2_MDL_REMOVE, l2_tei_remove},
1802         {ST_L2_8, EV_L2_MDL_REMOVE, l2_tei_remove},
1803         {ST_L2_4, EV_L2_SABME, l2_start_multi},
1804         {ST_L2_5, EV_L2_SABME, l2_send_UA},
1805         {ST_L2_6, EV_L2_SABME, l2_send_DM},
1806         {ST_L2_7, EV_L2_SABME, l2_restart_multi},
1807         {ST_L2_8, EV_L2_SABME, l2_restart_multi},
1808         {ST_L2_4, EV_L2_DISC, l2_send_DM},
1809         {ST_L2_5, EV_L2_DISC, l2_send_DM},
1810         {ST_L2_6, EV_L2_DISC, l2_send_UA},
1811         {ST_L2_7, EV_L2_DISC, l2_stop_multi},
1812         {ST_L2_8, EV_L2_DISC, l2_stop_multi},
1813         {ST_L2_4, EV_L2_UA, l2_mdl_error_ua},
1814         {ST_L2_5, EV_L2_UA, l2_connected},
1815         {ST_L2_6, EV_L2_UA, l2_released},
1816         {ST_L2_7, EV_L2_UA, l2_mdl_error_ua},
1817         {ST_L2_8, EV_L2_UA, l2_mdl_error_ua},
1818         {ST_L2_4, EV_L2_DM, l2_reestablish},
1819         {ST_L2_5, EV_L2_DM, l2_st5_dm_release},
1820         {ST_L2_6, EV_L2_DM, l2_st6_dm_release},
1821         {ST_L2_7, EV_L2_DM, l2_mdl_error_dm},
1822         {ST_L2_8, EV_L2_DM, l2_st8_mdl_error_dm},
1823         {ST_L2_1, EV_L2_UI, l2_got_ui},
1824         {ST_L2_2, EV_L2_UI, l2_got_ui},
1825         {ST_L2_3, EV_L2_UI, l2_got_ui},
1826         {ST_L2_4, EV_L2_UI, l2_got_ui},
1827         {ST_L2_5, EV_L2_UI, l2_got_ui},
1828         {ST_L2_6, EV_L2_UI, l2_got_ui},
1829         {ST_L2_7, EV_L2_UI, l2_got_ui},
1830         {ST_L2_8, EV_L2_UI, l2_got_ui},
1831         {ST_L2_7, EV_L2_FRMR, l2_got_FRMR},
1832         {ST_L2_8, EV_L2_FRMR, l2_got_FRMR},
1833         {ST_L2_7, EV_L2_SUPER, l2_st7_got_super},
1834         {ST_L2_8, EV_L2_SUPER, l2_st8_got_super},
1835         {ST_L2_7, EV_L2_I, l2_got_iframe},
1836         {ST_L2_8, EV_L2_I, l2_got_iframe},
1837         {ST_L2_5, EV_L2_T200, l2_timeout},
1838         {ST_L2_6, EV_L2_T200, l2_timeout},
1839         {ST_L2_7, EV_L2_T200, l2_timeout},
1840         {ST_L2_8, EV_L2_T200, l2_timeout},
1841         {ST_L2_7, EV_L2_T203, l2_timeout},
1842         {ST_L2_5, EV_L2_T200I, l2_st5_tout_200},
1843         {ST_L2_6, EV_L2_T200I, l2_st6_tout_200},
1844         {ST_L2_7, EV_L2_T200I, l2_st7_tout_200},
1845         {ST_L2_8, EV_L2_T200I, l2_st8_tout_200},
1846         {ST_L2_7, EV_L2_T203I, l2_st7_tout_203},
1847         {ST_L2_7, EV_L2_ACK_PULL, l2_pull_iqueue},
1848         {ST_L2_7, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
1849         {ST_L2_8, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
1850         {ST_L2_7, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy},
1851         {ST_L2_8, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy},
1852         {ST_L2_4, EV_L2_FRAME_ERROR, l2_frame_error},
1853         {ST_L2_5, EV_L2_FRAME_ERROR, l2_frame_error},
1854         {ST_L2_6, EV_L2_FRAME_ERROR, l2_frame_error},
1855         {ST_L2_7, EV_L2_FRAME_ERROR, l2_frame_error_reest},
1856         {ST_L2_8, EV_L2_FRAME_ERROR, l2_frame_error_reest},
1857         {ST_L2_1, EV_L1_DEACTIVATE, l2_st14_persistent_da},
1858         {ST_L2_2, EV_L1_DEACTIVATE, l2_st24_tei_remove},
1859         {ST_L2_3, EV_L1_DEACTIVATE, l2_st3_tei_remove},
1860         {ST_L2_4, EV_L1_DEACTIVATE, l2_st14_persistent_da},
1861         {ST_L2_5, EV_L1_DEACTIVATE, l2_st5_persistent_da},
1862         {ST_L2_6, EV_L1_DEACTIVATE, l2_st6_persistent_da},
1863         {ST_L2_7, EV_L1_DEACTIVATE, l2_persistent_da},
1864         {ST_L2_8, EV_L1_DEACTIVATE, l2_persistent_da},
1865 };
1866
1867 static int
1868 ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb)
1869 {
1870         u_char  *datap = skb->data;
1871         int     ret = -EINVAL;
1872         int     psapi, ptei;
1873         u_int   l;
1874         int     c = 0;
1875
1876         l = l2addrsize(l2);
1877         if (skb->len <= l) {
1878                 mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *) 'N');
1879                 return ret;
1880         }
1881         if (test_bit(FLG_LAPD, &l2->flag)) { /* Maybe not needed */
1882                 psapi = *datap++;
1883                 ptei = *datap++;
1884                 if ((psapi & 1) || !(ptei & 1)) {
1885                         printk(KERN_WARNING
1886                                "%s l2 D-channel frame wrong EA0/EA1\n",
1887                                mISDNDevName4ch(&l2->ch));
1888                         return ret;
1889                 }
1890                 psapi >>= 2;
1891                 ptei >>= 1;
1892                 if (psapi != l2->sapi) {
1893                         /* not our business */
1894                         if (*debug & DEBUG_L2)
1895                                 printk(KERN_DEBUG "%s: sapi %d/%d mismatch\n",
1896                                        mISDNDevName4ch(&l2->ch), psapi,
1897                                        l2->sapi);
1898                         dev_kfree_skb(skb);
1899                         return 0;
1900                 }
1901                 if ((ptei != l2->tei) && (ptei != GROUP_TEI)) {
1902                         /* not our business */
1903                         if (*debug & DEBUG_L2)
1904                                 printk(KERN_DEBUG "%s: tei %d/%d mismatch\n",
1905                                        mISDNDevName4ch(&l2->ch), ptei, l2->tei);
1906                         dev_kfree_skb(skb);
1907                         return 0;
1908                 }
1909         } else
1910                 datap += l;
1911         if (!(*datap & 1)) {    /* I-Frame */
1912                 c = iframe_error(l2, skb);
1913                 if (!c)
1914                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_I, skb);
1915         } else if (IsSFrame(datap, l2)) {       /* S-Frame */
1916                 c = super_error(l2, skb);
1917                 if (!c)
1918                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_SUPER, skb);
1919         } else if (IsUI(datap)) {
1920                 c = UI_error(l2, skb);
1921                 if (!c)
1922                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_UI, skb);
1923         } else if (IsSABME(datap, l2)) {
1924                 c = unnum_error(l2, skb, CMD);
1925                 if (!c)
1926                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_SABME, skb);
1927         } else if (IsUA(datap)) {
1928                 c = unnum_error(l2, skb, RSP);
1929                 if (!c)
1930                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_UA, skb);
1931         } else if (IsDISC(datap)) {
1932                 c = unnum_error(l2, skb, CMD);
1933                 if (!c)
1934                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DISC, skb);
1935         } else if (IsDM(datap)) {
1936                 c = unnum_error(l2, skb, RSP);
1937                 if (!c)
1938                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DM, skb);
1939         } else if (IsFRMR(datap)) {
1940                 c = FRMR_error(l2, skb);
1941                 if (!c)
1942                         ret = mISDN_FsmEvent(&l2->l2m, EV_L2_FRMR, skb);
1943         } else
1944                 c = 'L';
1945         if (c) {
1946                 printk(KERN_WARNING "%s:l2 D-channel frame error %c\n",
1947                        mISDNDevName4ch(&l2->ch), c);
1948                 mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *)(long)c);
1949         }
1950         return ret;
1951 }
1952
1953 static int
1954 l2_send(struct mISDNchannel *ch, struct sk_buff *skb)
1955 {
1956         struct layer2           *l2 = container_of(ch, struct layer2, ch);
1957         struct mISDNhead        *hh =  mISDN_HEAD_P(skb);
1958         int                     ret = -EINVAL;
1959
1960         if (*debug & DEBUG_L2_RECV)
1961                 printk(KERN_DEBUG "%s: %s prim(%x) id(%x) sapi(%d) tei(%d)\n",
1962                        __func__, mISDNDevName4ch(&l2->ch), hh->prim, hh->id,
1963                        l2->sapi, l2->tei);
1964         if (hh->prim == DL_INTERN_MSG) {
1965                 struct mISDNhead *chh = hh + 1; /* saved copy */
1966
1967                 *hh = *chh;
1968                 if (*debug & DEBUG_L2_RECV)
1969                         printk(KERN_DEBUG "%s: prim(%x) id(%x) internal msg\n",
1970                                 mISDNDevName4ch(&l2->ch), hh->prim, hh->id);
1971         }
1972         switch (hh->prim) {
1973         case PH_DATA_IND:
1974                 ret = ph_data_indication(l2, hh, skb);
1975                 break;
1976         case PH_DATA_CNF:
1977                 ret = ph_data_confirm(l2, hh, skb);
1978                 break;
1979         case PH_ACTIVATE_IND:
1980                 test_and_set_bit(FLG_L1_ACTIV, &l2->flag);
1981                 l2up_create(l2, MPH_ACTIVATE_IND, 0, NULL);
1982                 if (test_and_clear_bit(FLG_ESTAB_PEND, &l2->flag))
1983                         ret = mISDN_FsmEvent(&l2->l2m,
1984                                              EV_L2_DL_ESTABLISH_REQ, skb);
1985                 break;
1986         case PH_DEACTIVATE_IND:
1987                 test_and_clear_bit(FLG_L1_ACTIV, &l2->flag);
1988                 l2up_create(l2, MPH_DEACTIVATE_IND, 0, NULL);
1989                 ret = mISDN_FsmEvent(&l2->l2m, EV_L1_DEACTIVATE, skb);
1990                 break;
1991         case MPH_INFORMATION_IND:
1992                 if (!l2->up)
1993                         break;
1994                 ret = l2->up->send(l2->up, skb);
1995                 break;
1996         case DL_DATA_REQ:
1997                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_DATA, skb);
1998                 break;
1999         case DL_UNITDATA_REQ:
2000                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_UNITDATA, skb);
2001                 break;
2002         case DL_ESTABLISH_REQ:
2003                 if (test_bit(FLG_LAPB, &l2->flag))
2004                         test_and_set_bit(FLG_ORIG, &l2->flag);
2005                 if (test_bit(FLG_L1_ACTIV, &l2->flag)) {
2006                         if (test_bit(FLG_LAPD, &l2->flag) ||
2007                             test_bit(FLG_ORIG, &l2->flag))
2008                                 ret = mISDN_FsmEvent(&l2->l2m,
2009                                                      EV_L2_DL_ESTABLISH_REQ, skb);
2010                 } else {
2011                         if (test_bit(FLG_LAPD, &l2->flag) ||
2012                             test_bit(FLG_ORIG, &l2->flag)) {
2013                                 test_and_set_bit(FLG_ESTAB_PEND,
2014                                                  &l2->flag);
2015                         }
2016                         ret = l2down(l2, PH_ACTIVATE_REQ, l2_newid(l2),
2017                                      skb);
2018                 }
2019                 break;
2020         case DL_RELEASE_REQ:
2021                 if (test_bit(FLG_LAPB, &l2->flag))
2022                         l2down_create(l2, PH_DEACTIVATE_REQ,
2023                                       l2_newid(l2), 0, NULL);
2024                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_RELEASE_REQ,
2025                                      skb);
2026                 break;
2027         case DL_TIMER200_IND:
2028                 mISDN_FsmEvent(&l2->l2m, EV_L2_T200I, NULL);
2029                 break;
2030         case DL_TIMER203_IND:
2031                 mISDN_FsmEvent(&l2->l2m, EV_L2_T203I, NULL);
2032                 break;
2033         default:
2034                 if (*debug & DEBUG_L2)
2035                         l2m_debug(&l2->l2m, "l2 unknown pr %04x",
2036                                   hh->prim);
2037         }
2038         if (ret) {
2039                 dev_kfree_skb(skb);
2040                 ret = 0;
2041         }
2042         return ret;
2043 }
2044
2045 int
2046 tei_l2(struct layer2 *l2, u_int cmd, u_long arg)
2047 {
2048         int             ret = -EINVAL;
2049
2050         if (*debug & DEBUG_L2_TEI)
2051                 printk(KERN_DEBUG "%s: cmd(%x) in %s\n",
2052                        mISDNDevName4ch(&l2->ch), cmd, __func__);
2053         switch (cmd) {
2054         case (MDL_ASSIGN_REQ):
2055                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ASSIGN, (void *)arg);
2056                 break;
2057         case (MDL_REMOVE_REQ):
2058                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_REMOVE, NULL);
2059                 break;
2060         case (MDL_ERROR_IND):
2061                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL);
2062                 break;
2063         case (MDL_ERROR_RSP):
2064                 /* ETS 300-125 5.3.2.1 Test: TC13010 */
2065                 printk(KERN_NOTICE "%s: MDL_ERROR|REQ (tei_l2)\n",
2066                        mISDNDevName4ch(&l2->ch));
2067                 ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL);
2068                 break;
2069         }
2070         return ret;
2071 }
2072
2073 static void
2074 release_l2(struct layer2 *l2)
2075 {
2076         mISDN_FsmDelTimer(&l2->t200, 21);
2077         mISDN_FsmDelTimer(&l2->t203, 16);
2078         skb_queue_purge(&l2->i_queue);
2079         skb_queue_purge(&l2->ui_queue);
2080         skb_queue_purge(&l2->down_queue);
2081         ReleaseWin(l2);
2082         if (test_bit(FLG_LAPD, &l2->flag)) {
2083                 TEIrelease(l2);
2084                 if (l2->ch.st)
2085                         l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D,
2086                                                CLOSE_CHANNEL, NULL);
2087         }
2088         kfree(l2);
2089 }
2090
2091 static int
2092 l2_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
2093 {
2094         struct layer2           *l2 = container_of(ch, struct layer2, ch);
2095         u_int                   info;
2096
2097         if (*debug & DEBUG_L2_CTRL)
2098                 printk(KERN_DEBUG "%s: %s cmd(%x)\n",
2099                        mISDNDevName4ch(ch), __func__, cmd);
2100
2101         switch (cmd) {
2102         case OPEN_CHANNEL:
2103                 if (test_bit(FLG_LAPD, &l2->flag)) {
2104                         set_channel_address(&l2->ch, l2->sapi, l2->tei);
2105                         info = DL_INFO_L2_CONNECT;
2106                         l2up_create(l2, DL_INFORMATION_IND,
2107                                     sizeof(info), &info);
2108                 }
2109                 break;
2110         case CLOSE_CHANNEL:
2111                 if (l2->ch.peer)
2112                         l2->ch.peer->ctrl(l2->ch.peer, CLOSE_CHANNEL, NULL);
2113                 release_l2(l2);
2114                 break;
2115         }
2116         return 0;
2117 }
2118
2119 struct layer2 *
2120 create_l2(struct mISDNchannel *ch, u_int protocol, u_long options, int tei,
2121           int sapi)
2122 {
2123         struct layer2           *l2;
2124         struct channel_req      rq;
2125
2126         l2 = kzalloc(sizeof(struct layer2), GFP_KERNEL);
2127         if (!l2) {
2128                 printk(KERN_ERR "kzalloc layer2 failed\n");
2129                 return NULL;
2130         }
2131         l2->next_id = 1;
2132         l2->down_id = MISDN_ID_NONE;
2133         l2->up = ch;
2134         l2->ch.st = ch->st;
2135         l2->ch.send = l2_send;
2136         l2->ch.ctrl = l2_ctrl;
2137         switch (protocol) {
2138         case ISDN_P_LAPD_NT:
2139                 test_and_set_bit(FLG_LAPD, &l2->flag);
2140                 test_and_set_bit(FLG_LAPD_NET, &l2->flag);
2141                 test_and_set_bit(FLG_MOD128, &l2->flag);
2142                 l2->sapi = sapi;
2143                 l2->maxlen = MAX_DFRAME_LEN;
2144                 if (test_bit(OPTION_L2_PMX, &options))
2145                         l2->window = 7;
2146                 else
2147                         l2->window = 1;
2148                 if (test_bit(OPTION_L2_PTP, &options))
2149                         test_and_set_bit(FLG_PTP, &l2->flag);
2150                 if (test_bit(OPTION_L2_FIXEDTEI, &options))
2151                         test_and_set_bit(FLG_FIXED_TEI, &l2->flag);
2152                 l2->tei = tei;
2153                 l2->T200 = 1000;
2154                 l2->N200 = 3;
2155                 l2->T203 = 10000;
2156                 if (test_bit(OPTION_L2_PMX, &options))
2157                         rq.protocol = ISDN_P_NT_E1;
2158                 else
2159                         rq.protocol = ISDN_P_NT_S0;
2160                 rq.adr.channel = 0;
2161                 l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, OPEN_CHANNEL, &rq);
2162                 break;
2163         case ISDN_P_LAPD_TE:
2164                 test_and_set_bit(FLG_LAPD, &l2->flag);
2165                 test_and_set_bit(FLG_MOD128, &l2->flag);
2166                 test_and_set_bit(FLG_ORIG, &l2->flag);
2167                 l2->sapi = sapi;
2168                 l2->maxlen = MAX_DFRAME_LEN;
2169                 if (test_bit(OPTION_L2_PMX, &options))
2170                         l2->window = 7;
2171                 else
2172                         l2->window = 1;
2173                 if (test_bit(OPTION_L2_PTP, &options))
2174                         test_and_set_bit(FLG_PTP, &l2->flag);
2175                 if (test_bit(OPTION_L2_FIXEDTEI, &options))
2176                         test_and_set_bit(FLG_FIXED_TEI, &l2->flag);
2177                 l2->tei = tei;
2178                 l2->T200 = 1000;
2179                 l2->N200 = 3;
2180                 l2->T203 = 10000;
2181                 if (test_bit(OPTION_L2_PMX, &options))
2182                         rq.protocol = ISDN_P_TE_E1;
2183                 else
2184                         rq.protocol = ISDN_P_TE_S0;
2185                 rq.adr.channel = 0;
2186                 l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, OPEN_CHANNEL, &rq);
2187                 break;
2188         case ISDN_P_B_X75SLP:
2189                 test_and_set_bit(FLG_LAPB, &l2->flag);
2190                 l2->window = 7;
2191                 l2->maxlen = MAX_DATA_SIZE;
2192                 l2->T200 = 1000;
2193                 l2->N200 = 4;
2194                 l2->T203 = 5000;
2195                 l2->addr.A = 3;
2196                 l2->addr.B = 1;
2197                 break;
2198         default:
2199                 printk(KERN_ERR "layer2 create failed prt %x\n",
2200                        protocol);
2201                 kfree(l2);
2202                 return NULL;
2203         }
2204         skb_queue_head_init(&l2->i_queue);
2205         skb_queue_head_init(&l2->ui_queue);
2206         skb_queue_head_init(&l2->down_queue);
2207         skb_queue_head_init(&l2->tmp_queue);
2208         InitWin(l2);
2209         l2->l2m.fsm = &l2fsm;
2210         if (test_bit(FLG_LAPB, &l2->flag) ||
2211             test_bit(FLG_FIXED_TEI, &l2->flag) ||
2212             test_bit(FLG_LAPD_NET, &l2->flag))
2213                 l2->l2m.state = ST_L2_4;
2214         else
2215                 l2->l2m.state = ST_L2_1;
2216         l2->l2m.debug = *debug;
2217         l2->l2m.userdata = l2;
2218         l2->l2m.userint = 0;
2219         l2->l2m.printdebug = l2m_debug;
2220
2221         mISDN_FsmInitTimer(&l2->l2m, &l2->t200);
2222         mISDN_FsmInitTimer(&l2->l2m, &l2->t203);
2223         return l2;
2224 }
2225
2226 static int
2227 x75create(struct channel_req *crq)
2228 {
2229         struct layer2   *l2;
2230
2231         if (crq->protocol != ISDN_P_B_X75SLP)
2232                 return -EPROTONOSUPPORT;
2233         l2 = create_l2(crq->ch, crq->protocol, 0, 0, 0);
2234         if (!l2)
2235                 return -ENOMEM;
2236         crq->ch = &l2->ch;
2237         crq->protocol = ISDN_P_B_HDLC;
2238         return 0;
2239 }
2240
2241 static struct Bprotocol X75SLP = {
2242         .Bprotocols = (1 << (ISDN_P_B_X75SLP & ISDN_P_B_MASK)),
2243         .name = "X75SLP",
2244         .create = x75create
2245 };
2246
2247 int
2248 Isdnl2_Init(u_int *deb)
2249 {
2250         debug = deb;
2251         mISDN_register_Bprotocol(&X75SLP);
2252         l2fsm.state_count = L2_STATE_COUNT;
2253         l2fsm.event_count = L2_EVENT_COUNT;
2254         l2fsm.strEvent = strL2Event;
2255         l2fsm.strState = strL2State;
2256         mISDN_FsmNew(&l2fsm, L2FnList, ARRAY_SIZE(L2FnList));
2257         TEIInit(deb);
2258         return 0;
2259 }
2260
2261 void
2262 Isdnl2_cleanup(void)
2263 {
2264         mISDN_unregister_Bprotocol(&X75SLP);
2265         TEIFree();
2266         mISDN_FsmFree(&l2fsm);
2267 }