drivers/isdn/mISDN/tei.c

   1 /*
   2  *
   3  * Author       Karsten Keil <[email protected]>
   4  *
   5  * Copyright 2008  by Karsten Keil <[email protected]>
   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 #include "layer2.h"
  18 #include <linux/random.h>
  19 #include <linux/slab.h>
  20 #include "core.h"
  21
  22 #define ID_REQUEST      1
  23 #define ID_ASSIGNED     2
  24 #define ID_DENIED       3
  25 #define ID_CHK_REQ      4
  26 #define ID_CHK_RES      5
  27 #define ID_REMOVE       6
  28 #define ID_VERIFY       7
  29
  30 #define TEI_ENTITY_ID   0xf
  31
  32 #define MGR_PH_ACTIVE   16
  33 #define MGR_PH_NOTREADY 17
  34
  35 #define DATIMER_VAL     10000
  36
  37 static  u_int   *debug;
  38
  39 static struct Fsm deactfsm = {NULL, 0, 0, NULL, NULL};
  40 static struct Fsm teifsmu = {NULL, 0, 0, NULL, NULL};
  41 static struct Fsm teifsmn = {NULL, 0, 0, NULL, NULL};
  42
  43 enum {
  44         ST_L1_DEACT,
  45         ST_L1_DEACT_PENDING,
  46         ST_L1_ACTIV,
  47 };
  48 #define DEACT_STATE_COUNT (ST_L1_ACTIV + 1)
  49
  50 static char *strDeactState[] =
  51 {
  52         "ST_L1_DEACT",
  53         "ST_L1_DEACT_PENDING",
  54         "ST_L1_ACTIV",
  55 };
  56
  57 enum {
  58         EV_ACTIVATE,
  59         EV_ACTIVATE_IND,
  60         EV_DEACTIVATE,
  61         EV_DEACTIVATE_IND,
  62         EV_UI,
  63         EV_DATIMER,
  64 };
  65
  66 #define DEACT_EVENT_COUNT (EV_DATIMER + 1)
  67
  68 static char *strDeactEvent[] =
  69 {
  70         "EV_ACTIVATE",
  71         "EV_ACTIVATE_IND",
  72         "EV_DEACTIVATE",
  73         "EV_DEACTIVATE_IND",
  74         "EV_UI",
  75         "EV_DATIMER",
  76 };
  77
  78 static void
  79 da_debug(struct FsmInst *fi, char *fmt, ...)
  80 {
  81         struct manager  *mgr = fi->userdata;
  82         struct va_format vaf;
  83         va_list va;
  84
  85         if (!(*debug & DEBUG_L2_TEIFSM))
  86                 return;
  87
  88         va_start(va, fmt);
  89
  90         vaf.fmt = fmt;
  91         vaf.va = &va;
  92
  93         printk(KERN_DEBUG "mgr(%d): %pV\n", mgr->ch.st->dev->id, &vaf);
  94
  95         va_end(va);
  96 }
  97
  98 static void
  99 da_activate(struct FsmInst *fi, int event, void *arg)
 100 {
 101         struct manager  *mgr = fi->userdata;
 102
 103         if (fi->state == ST_L1_DEACT_PENDING)
 104                 mISDN_FsmDelTimer(&mgr->datimer, 1);
 105         mISDN_FsmChangeState(fi, ST_L1_ACTIV);
 106 }
 107
 108 static void
 109 da_deactivate_ind(struct FsmInst *fi, int event, void *arg)
 110 {
 111         mISDN_FsmChangeState(fi, ST_L1_DEACT);
 112 }
 113
 114 static void
 115 da_deactivate(struct FsmInst *fi, int event, void *arg)
 116 {
 117         struct manager  *mgr = fi->userdata;
 118         struct layer2   *l2;
 119         u_long          flags;
 120
 121         read_lock_irqsave(&mgr->lock, flags);
 122         list_for_each_entry(l2, &mgr->layer2, list) {
 123                 if (l2->l2m.state > ST_L2_4) {
 124                         /* have still activ TEI */
 125                         read_unlock_irqrestore(&mgr->lock, flags);
 126                         return;
 127                 }
 128         }
 129         read_unlock_irqrestore(&mgr->lock, flags);
 130         /* All TEI are inactiv */
 131         if (!test_bit(OPTION_L1_HOLD, &mgr->options)) {
 132                 mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER,
 133                                   NULL, 1);
 134                 mISDN_FsmChangeState(fi, ST_L1_DEACT_PENDING);
 135         }
 136 }
 137
 138 static void
 139 da_ui(struct FsmInst *fi, int event, void *arg)
 140 {
 141         struct manager  *mgr = fi->userdata;
 142
 143         /* restart da timer */
 144         if (!test_bit(OPTION_L1_HOLD, &mgr->options)) {
 145                 mISDN_FsmDelTimer(&mgr->datimer, 2);
 146                 mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER,
 147                                   NULL, 2);
 148         }
 149 }
 150
 151 static void
 152 da_timer(struct FsmInst *fi, int event, void *arg)
 153 {
 154         struct manager  *mgr = fi->userdata;
 155         struct layer2   *l2;
 156         u_long          flags;
 157
 158         /* check again */
 159         read_lock_irqsave(&mgr->lock, flags);
 160         list_for_each_entry(l2, &mgr->layer2, list) {
 161                 if (l2->l2m.state > ST_L2_4) {
 162                         /* have still activ TEI */
 163                         read_unlock_irqrestore(&mgr->lock, flags);
 164                         mISDN_FsmChangeState(fi, ST_L1_ACTIV);
 165                         return;
 166                 }
 167         }
 168         read_unlock_irqrestore(&mgr->lock, flags);
 169         /* All TEI are inactiv */
 170         mISDN_FsmChangeState(fi, ST_L1_DEACT);
 171         _queue_data(&mgr->ch, PH_DEACTIVATE_REQ, MISDN_ID_ANY, 0, NULL,
 172                     GFP_ATOMIC);
 173 }
 174
 175 static struct FsmNode DeactFnList[] =
 176 {
 177         {ST_L1_DEACT, EV_ACTIVATE_IND, da_activate},
 178         {ST_L1_ACTIV, EV_DEACTIVATE_IND, da_deactivate_ind},
 179         {ST_L1_ACTIV, EV_DEACTIVATE, da_deactivate},
 180         {ST_L1_DEACT_PENDING, EV_ACTIVATE, da_activate},
 181         {ST_L1_DEACT_PENDING, EV_UI, da_ui},
 182         {ST_L1_DEACT_PENDING, EV_DATIMER, da_timer},
 183 };
 184
 185 enum {
 186         ST_TEI_NOP,
 187         ST_TEI_IDREQ,
 188         ST_TEI_IDVERIFY,
 189 };
 190
 191 #define TEI_STATE_COUNT (ST_TEI_IDVERIFY + 1)
 192
 193 static char *strTeiState[] =
 194 {
 195         "ST_TEI_NOP",
 196         "ST_TEI_IDREQ",
 197         "ST_TEI_IDVERIFY",
 198 };
 199
 200 enum {
 201         EV_IDREQ,
 202         EV_ASSIGN,
 203         EV_ASSIGN_REQ,
 204         EV_DENIED,
 205         EV_CHKREQ,
 206         EV_CHKRESP,
 207         EV_REMOVE,
 208         EV_VERIFY,
 209         EV_TIMER,
 210 };
 211
 212 #define TEI_EVENT_COUNT (EV_TIMER + 1)
 213
 214 static char *strTeiEvent[] =
 215 {
 216         "EV_IDREQ",
 217         "EV_ASSIGN",
 218         "EV_ASSIGN_REQ",
 219         "EV_DENIED",
 220         "EV_CHKREQ",
 221         "EV_CHKRESP",
 222         "EV_REMOVE",
 223         "EV_VERIFY",
 224         "EV_TIMER",
 225 };
 226
 227 static void
 228 tei_debug(struct FsmInst *fi, char *fmt, ...)
 229 {
 230         struct teimgr   *tm = fi->userdata;
 231         struct va_format vaf;
 232         va_list va;
 233
 234         if (!(*debug & DEBUG_L2_TEIFSM))
 235                 return;
 236
 237         va_start(va, fmt);
 238
 239         vaf.fmt = fmt;
 240         vaf.va = &va;
 241
 242         printk(KERN_DEBUG "sapi(%d) tei(%d): %pV\n",
 243                tm->l2->sapi, tm->l2->tei, &vaf);
 244
 245         va_end(va);
 246 }
 247
 248
 249
 250 static int
 251 get_free_id(struct manager *mgr)
 252 {
 253         DECLARE_BITMAP(ids, 64) = { [0 ... BITS_TO_LONGS(64) - 1] = 0 };
 254         int             i;
 255         struct layer2   *l2;
 256
 257         list_for_each_entry(l2, &mgr->layer2, list) {
 258                 if (l2->ch.nr > 63) {
 259                         printk(KERN_WARNING
 260                                "%s: more as 63 layer2 for one device\n",
 261                                __func__);
 262                         return -EBUSY;
 263                 }
 264                 __set_bit(l2->ch.nr, ids);
 265         }
 266         i = find_next_zero_bit(ids, 64, 1);
 267         if (i < 64)
 268                 return i;
 269         printk(KERN_WARNING "%s: more as 63 layer2 for one device\n",
 270                __func__);
 271         return -EBUSY;
 272 }
 273
 274 static int
 275 get_free_tei(struct manager *mgr)
 276 {
 277         DECLARE_BITMAP(ids, 64) = { [0 ... BITS_TO_LONGS(64) - 1] = 0 };
 278         int             i;
 279         struct layer2   *l2;
 280
 281         list_for_each_entry(l2, &mgr->layer2, list) {
 282                 if (l2->ch.nr == 0)
 283                         continue;
 284                 if ((l2->ch.addr & 0xff) != 0)
 285                         continue;
 286                 i = l2->ch.addr >> 8;
 287                 if (i < 64)
 288                         continue;
 289                 i -= 64;
 290
 291                 __set_bit(i, ids);
 292         }
 293         i = find_first_zero_bit(ids, 64);
 294         if (i < 64)
 295                 return i + 64;
 296         printk(KERN_WARNING "%s: more as 63 dynamic tei for one device\n",
 297                __func__);
 298         return -1;
 299 }
 300
 301 static void
 302 teiup_create(struct manager *mgr, u_int prim, int len, void *arg)
 303 {
 304         struct sk_buff  *skb;
 305         struct mISDNhead *hh;
 306         int             err;
 307
 308         skb = mI_alloc_skb(len, GFP_ATOMIC);
 309         if (!skb)
 310                 return;
 311         hh = mISDN_HEAD_P(skb);
 312         hh->prim = prim;
 313         hh->id = (mgr->ch.nr << 16) | mgr->ch.addr;
 314         if (len)
 315                 skb_put_data(skb, arg, len);
 316         err = mgr->up->send(mgr->up, skb);
 317         if (err) {
 318                 printk(KERN_WARNING "%s: err=%d\n", __func__, err);
 319                 dev_kfree_skb(skb);
 320         }
 321 }
 322
 323 static u_int
 324 new_id(struct manager *mgr)
 325 {
 326         u_int   id;
 327
 328         id = mgr->nextid++;
 329         if (id == 0x7fff)
 330                 mgr->nextid = 1;
 331         id <<= 16;
 332         id |= GROUP_TEI << 8;
 333         id |= TEI_SAPI;
 334         return id;
 335 }
 336
 337 static void
 338 do_send(struct manager *mgr)
 339 {
 340         if (!test_bit(MGR_PH_ACTIVE, &mgr->options))
 341                 return;
 342
 343         if (!test_and_set_bit(MGR_PH_NOTREADY, &mgr->options)) {
 344                 struct sk_buff  *skb = skb_dequeue(&mgr->sendq);
 345
 346                 if (!skb) {
 347                         test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
 348                         return;
 349                 }
 350                 mgr->lastid = mISDN_HEAD_ID(skb);
 351                 mISDN_FsmEvent(&mgr->deact, EV_UI, NULL);
 352                 if (mgr->ch.recv(mgr->ch.peer, skb)) {
 353                         dev_kfree_skb(skb);
 354                         test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
 355                         mgr->lastid = MISDN_ID_NONE;
 356                 }
 357         }
 358 }
 359
 360 static void
 361 do_ack(struct manager *mgr, u_int id)
 362 {
 363         if (test_bit(MGR_PH_NOTREADY, &mgr->options)) {
 364                 if (id == mgr->lastid) {
 365                         if (test_bit(MGR_PH_ACTIVE, &mgr->options)) {
 366                                 struct sk_buff  *skb;
 367
 368                                 skb = skb_dequeue(&mgr->sendq);
 369                                 if (skb) {
 370                                         mgr->lastid = mISDN_HEAD_ID(skb);
 371                                         if (!mgr->ch.recv(mgr->ch.peer, skb))
 372                                                 return;
 373                                         dev_kfree_skb(skb);
 374                                 }
 375                         }
 376                         mgr->lastid = MISDN_ID_NONE;
 377                         test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
 378                 }
 379         }
 380 }
 381
 382 static void
 383 mgr_send_down(struct manager *mgr, struct sk_buff *skb)
 384 {
 385         skb_queue_tail(&mgr->sendq, skb);
 386         if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) {
 387                 _queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0,
 388                             NULL, GFP_KERNEL);
 389         } else {
 390                 do_send(mgr);
 391         }
 392 }
 393
 394 static int
 395 dl_unit_data(struct manager *mgr, struct sk_buff *skb)
 396 {
 397         if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) /* only net send UI */
 398                 return -EINVAL;
 399         if (!test_bit(MGR_PH_ACTIVE, &mgr->options))
 400                 _queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0,
 401                             NULL, GFP_KERNEL);
 402         skb_push(skb, 3);
 403         skb->data[0] = 0x02; /* SAPI 0 C/R = 1 */
 404         skb->data[1] = 0xff; /* TEI 127 */
 405         skb->data[2] = UI;   /* UI frame */
 406         mISDN_HEAD_PRIM(skb) = PH_DATA_REQ;
 407         mISDN_HEAD_ID(skb) = new_id(mgr);
 408         skb_queue_tail(&mgr->sendq, skb);
 409         do_send(mgr);
 410         return 0;
 411 }
 412
 413 static unsigned int
 414 random_ri(void)
 415 {
 416         u16 x;
 417
 418         get_random_bytes(&x, sizeof(x));
 419         return x;
 420 }
 421
 422 static struct layer2 *
 423 findtei(struct manager *mgr, int tei)
 424 {
 425         struct layer2   *l2;
 426         u_long          flags;
 427
 428         read_lock_irqsave(&mgr->lock, flags);
 429         list_for_each_entry(l2, &mgr->layer2, list) {
 430                 if ((l2->sapi == 0) && (l2->tei > 0) &&
 431                     (l2->tei != GROUP_TEI) && (l2->tei == tei))
 432                         goto done;
 433         }
 434         l2 = NULL;
 435 done:
 436         read_unlock_irqrestore(&mgr->lock, flags);
 437         return l2;
 438 }
 439
 440 static void
 441 put_tei_msg(struct manager *mgr, u_char m_id, unsigned int ri, int tei)
 442 {
 443         struct sk_buff *skb;
 444         u_char bp[8];
 445
 446         bp[0] = (TEI_SAPI << 2);
 447         if (test_bit(MGR_OPT_NETWORK, &mgr->options))
 448                 bp[0] |= 2; /* CR:=1 for net command */
 449         bp[1] = (GROUP_TEI << 1) | 0x1;
 450         bp[2] = UI;
 451         bp[3] = TEI_ENTITY_ID;
 452         bp[4] = ri >> 8;
 453         bp[5] = ri & 0xff;
 454         bp[6] = m_id;
 455         bp[7] = ((tei << 1) & 0xff) | 1;
 456         skb = _alloc_mISDN_skb(PH_DATA_REQ, new_id(mgr), 8, bp, GFP_ATOMIC);
 457         if (!skb) {
 458                 printk(KERN_WARNING "%s: no skb for tei msg\n", __func__);
 459                 return;
 460         }
 461         mgr_send_down(mgr, skb);
 462 }
 463
 464 static void
 465 tei_id_request(struct FsmInst *fi, int event, void *arg)
 466 {
 467         struct teimgr *tm = fi->userdata;
 468
 469         if (tm->l2->tei != GROUP_TEI) {
 470                 tm->tei_m.printdebug(&tm->tei_m,
 471                                      "assign request for already assigned tei %d",
 472                                      tm->l2->tei);
 473                 return;
 474         }
 475         tm->ri = random_ri();
 476         if (*debug & DEBUG_L2_TEI)
 477                 tm->tei_m.printdebug(&tm->tei_m,
 478                                      "assign request ri %d", tm->ri);
 479         put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI);
 480         mISDN_FsmChangeState(fi, ST_TEI_IDREQ);
 481         mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 1);
 482         tm->nval = 3;
 483 }
 484
 485 static void
 486 tei_id_assign(struct FsmInst *fi, int event, void *arg)
 487 {
 488         struct teimgr   *tm = fi->userdata;
 489         struct layer2   *l2;
 490         u_char *dp = arg;
 491         int ri, tei;
 492
 493         ri = ((unsigned int) *dp++ << 8);
 494         ri += *dp++;
 495         dp++;
 496         tei = *dp >> 1;
 497         if (*debug & DEBUG_L2_TEI)
 498                 tm->tei_m.printdebug(fi, "identity assign ri %d tei %d",
 499                                      ri, tei);
 500         l2 = findtei(tm->mgr, tei);
 501         if (l2) {       /* same tei is in use */
 502                 if (ri != l2->tm->ri) {
 503                         tm->tei_m.printdebug(fi,
 504                                              "possible duplicate assignment tei %d", tei);
 505                         tei_l2(l2, MDL_ERROR_RSP, 0);
 506                 }
 507         } else if (ri == tm->ri) {
 508                 mISDN_FsmDelTimer(&tm->timer, 1);
 509                 mISDN_FsmChangeState(fi, ST_TEI_NOP);
 510                 tei_l2(tm->l2, MDL_ASSIGN_REQ, tei);
 511         }
 512 }
 513
 514 static void
 515 tei_id_test_dup(struct FsmInst *fi, int event, void *arg)
 516 {
 517         struct teimgr   *tm = fi->userdata;
 518         struct layer2   *l2;
 519         u_char *dp = arg;
 520         int tei, ri;
 521
 522         ri = ((unsigned int) *dp++ << 8);
 523         ri += *dp++;
 524         dp++;
 525         tei = *dp >> 1;
 526         if (*debug & DEBUG_L2_TEI)
 527                 tm->tei_m.printdebug(fi, "foreign identity assign ri %d tei %d",
 528                                      ri, tei);
 529         l2 = findtei(tm->mgr, tei);
 530         if (l2) {       /* same tei is in use */
 531                 if (ri != l2->tm->ri) { /* and it wasn't our request */
 532                         tm->tei_m.printdebug(fi,
 533                                              "possible duplicate assignment tei %d", tei);
 534                         mISDN_FsmEvent(&l2->tm->tei_m, EV_VERIFY, NULL);
 535                 }
 536         }
 537 }
 538
 539 static void
 540 tei_id_denied(struct FsmInst *fi, int event, void *arg)
 541 {
 542         struct teimgr *tm = fi->userdata;
 543         u_char *dp = arg;
 544         int ri, tei;
 545
 546         ri = ((unsigned int) *dp++ << 8);
 547         ri += *dp++;
 548         dp++;
 549         tei = *dp >> 1;
 550         if (*debug & DEBUG_L2_TEI)
 551                 tm->tei_m.printdebug(fi, "identity denied ri %d tei %d",
 552                                      ri, tei);
 553 }
 554
 555 static void
 556 tei_id_chk_req(struct FsmInst *fi, int event, void *arg)
 557 {
 558         struct teimgr *tm = fi->userdata;
 559         u_char *dp = arg;
 560         int tei;
 561
 562         tei = *(dp + 3) >> 1;
 563         if (*debug & DEBUG_L2_TEI)
 564                 tm->tei_m.printdebug(fi, "identity check req tei %d", tei);
 565         if ((tm->l2->tei != GROUP_TEI) && ((tei == GROUP_TEI) ||
 566                                            (tei == tm->l2->tei))) {
 567                 mISDN_FsmDelTimer(&tm->timer, 4);
 568                 mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP);
 569                 put_tei_msg(tm->mgr, ID_CHK_RES, random_ri(), tm->l2->tei);
 570         }
 571 }
 572
 573 static void
 574 tei_id_remove(struct FsmInst *fi, int event, void *arg)
 575 {
 576         struct teimgr *tm = fi->userdata;
 577         u_char *dp = arg;
 578         int tei;
 579
 580         tei = *(dp + 3) >> 1;
 581         if (*debug & DEBUG_L2_TEI)
 582                 tm->tei_m.printdebug(fi, "identity remove tei %d", tei);
 583         if ((tm->l2->tei != GROUP_TEI) &&
 584             ((tei == GROUP_TEI) || (tei == tm->l2->tei))) {
 585                 mISDN_FsmDelTimer(&tm->timer, 5);
 586                 mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP);
 587                 tei_l2(tm->l2, MDL_REMOVE_REQ, 0);
 588         }
 589 }
 590
 591 static void
 592 tei_id_verify(struct FsmInst *fi, int event, void *arg)
 593 {
 594         struct teimgr *tm = fi->userdata;
 595
 596         if (*debug & DEBUG_L2_TEI)
 597                 tm->tei_m.printdebug(fi, "id verify request for tei %d",
 598                                      tm->l2->tei);
 599         put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei);
 600         mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY);
 601         mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2);
 602         tm->nval = 2;
 603 }
 604
 605 static void
 606 tei_id_req_tout(struct FsmInst *fi, int event, void *arg)
 607 {
 608         struct teimgr *tm = fi->userdata;
 609
 610         if (--tm->nval) {
 611                 tm->ri = random_ri();
 612                 if (*debug & DEBUG_L2_TEI)
 613                         tm->tei_m.printdebug(fi, "assign req(%d) ri %d",
 614                                              4 - tm->nval, tm->ri);
 615                 put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI);
 616                 mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 3);
 617         } else {
 618                 tm->tei_m.printdebug(fi, "assign req failed");
 619                 tei_l2(tm->l2, MDL_ERROR_RSP, 0);
 620                 mISDN_FsmChangeState(fi, ST_TEI_NOP);
 621         }
 622 }
 623
 624 static void
 625 tei_id_ver_tout(struct FsmInst *fi, int event, void *arg)
 626 {
 627         struct teimgr *tm = fi->userdata;
 628
 629         if (--tm->nval) {
 630                 if (*debug & DEBUG_L2_TEI)
 631                         tm->tei_m.printdebug(fi,
 632                                              "id verify req(%d) for tei %d",
 633                                              3 - tm->nval, tm->l2->tei);
 634                 put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei);
 635                 mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4);
 636         } else {
 637                 tm->tei_m.printdebug(fi, "verify req for tei %d failed",
 638                                      tm->l2->tei);
 639                 tei_l2(tm->l2, MDL_REMOVE_REQ, 0);
 640                 mISDN_FsmChangeState(fi, ST_TEI_NOP);
 641         }
 642 }
 643
 644 static struct FsmNode TeiFnListUser[] =
 645 {
 646         {ST_TEI_NOP, EV_IDREQ, tei_id_request},
 647         {ST_TEI_NOP, EV_ASSIGN, tei_id_test_dup},
 648         {ST_TEI_NOP, EV_VERIFY, tei_id_verify},
 649         {ST_TEI_NOP, EV_REMOVE, tei_id_remove},
 650         {ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req},
 651         {ST_TEI_IDREQ, EV_TIMER, tei_id_req_tout},
 652         {ST_TEI_IDREQ, EV_ASSIGN, tei_id_assign},
 653         {ST_TEI_IDREQ, EV_DENIED, tei_id_denied},
 654         {ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout},
 655         {ST_TEI_IDVERIFY, EV_REMOVE, tei_id_remove},
 656         {ST_TEI_IDVERIFY, EV_CHKREQ, tei_id_chk_req},
 657 };
 658
 659 static void
 660 tei_l2remove(struct layer2 *l2)
 661 {
 662         put_tei_msg(l2->tm->mgr, ID_REMOVE, 0, l2->tei);
 663         tei_l2(l2, MDL_REMOVE_REQ, 0);
 664         list_del(&l2->ch.list);
 665         l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
 666 }
 667
 668 static void
 669 tei_assign_req(struct FsmInst *fi, int event, void *arg)
 670 {
 671         struct teimgr *tm = fi->userdata;
 672         u_char *dp = arg;
 673
 674         if (tm->l2->tei == GROUP_TEI) {
 675                 tm->tei_m.printdebug(&tm->tei_m,
 676                                      "net tei assign request without tei");
 677                 return;
 678         }
 679         tm->ri = ((unsigned int) *dp++ << 8);
 680         tm->ri += *dp++;
 681         if (*debug & DEBUG_L2_TEI)
 682                 tm->tei_m.printdebug(&tm->tei_m,
 683                                      "net assign request ri %d teim %d", tm->ri, *dp);
 684         put_tei_msg(tm->mgr, ID_ASSIGNED, tm->ri, tm->l2->tei);
 685         mISDN_FsmChangeState(fi, ST_TEI_NOP);
 686 }
 687
 688 static void
 689 tei_id_chk_req_net(struct FsmInst *fi, int event, void *arg)
 690 {
 691         struct teimgr   *tm = fi->userdata;
 692
 693         if (*debug & DEBUG_L2_TEI)
 694                 tm->tei_m.printdebug(fi, "id check request for tei %d",
 695                                      tm->l2->tei);
 696         tm->rcnt = 0;
 697         put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei);
 698         mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY);
 699         mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2);
 700         tm->nval = 2;
 701 }
 702
 703 static void
 704 tei_id_chk_resp(struct FsmInst *fi, int event, void *arg)
 705 {
 706         struct teimgr *tm = fi->userdata;
 707         u_char *dp = arg;
 708         int tei;
 709
 710         tei = dp[3] >> 1;
 711         if (*debug & DEBUG_L2_TEI)
 712                 tm->tei_m.printdebug(fi, "identity check resp tei %d", tei);
 713         if (tei == tm->l2->tei)
 714                 tm->rcnt++;
 715 }
 716
 717 static void
 718 tei_id_verify_net(struct FsmInst *fi, int event, void *arg)
 719 {
 720         struct teimgr *tm = fi->userdata;
 721         u_char *dp = arg;
 722         int tei;
 723
 724         tei = dp[3] >> 1;
 725         if (*debug & DEBUG_L2_TEI)
 726                 tm->tei_m.printdebug(fi, "identity verify req tei %d/%d",
 727                                      tei, tm->l2->tei);
 728         if (tei == tm->l2->tei)
 729                 tei_id_chk_req_net(fi, event, arg);
 730 }
 731
 732 static void
 733 tei_id_ver_tout_net(struct FsmInst *fi, int event, void *arg)
 734 {
 735         struct teimgr *tm = fi->userdata;
 736
 737         if (tm->rcnt == 1) {
 738                 if (*debug & DEBUG_L2_TEI)
 739                         tm->tei_m.printdebug(fi,
 740                                              "check req for tei %d successful\n", tm->l2->tei);
 741                 mISDN_FsmChangeState(fi, ST_TEI_NOP);
 742         } else if (tm->rcnt > 1) {
 743                 /* duplicate assignment; remove */
 744                 tei_l2remove(tm->l2);
 745         } else if (--tm->nval) {
 746                 if (*debug & DEBUG_L2_TEI)
 747                         tm->tei_m.printdebug(fi,
 748                                              "id check req(%d) for tei %d",
 749                                              3 - tm->nval, tm->l2->tei);
 750                 put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei);
 751                 mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4);
 752         } else {
 753                 tm->tei_m.printdebug(fi, "check req for tei %d failed",
 754                                      tm->l2->tei);
 755                 mISDN_FsmChangeState(fi, ST_TEI_NOP);
 756                 tei_l2remove(tm->l2);
 757         }
 758 }
 759
 760 static struct FsmNode TeiFnListNet[] =
 761 {
 762         {ST_TEI_NOP, EV_ASSIGN_REQ, tei_assign_req},
 763         {ST_TEI_NOP, EV_VERIFY, tei_id_verify_net},
 764         {ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req_net},
 765         {ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout_net},
 766         {ST_TEI_IDVERIFY, EV_CHKRESP, tei_id_chk_resp},
 767 };
 768
 769 static void
 770 tei_ph_data_ind(struct teimgr *tm, u_int mt, u_char *dp, int len)
 771 {
 772         if (test_bit(FLG_FIXED_TEI, &tm->l2->flag))
 773                 return;
 774         if (*debug & DEBUG_L2_TEI)
 775                 tm->tei_m.printdebug(&tm->tei_m, "tei handler mt %x", mt);
 776         if (mt == ID_ASSIGNED)
 777                 mISDN_FsmEvent(&tm->tei_m, EV_ASSIGN, dp);
 778         else if (mt == ID_DENIED)
 779                 mISDN_FsmEvent(&tm->tei_m, EV_DENIED, dp);
 780         else if (mt == ID_CHK_REQ)
 781                 mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, dp);
 782         else if (mt == ID_REMOVE)
 783                 mISDN_FsmEvent(&tm->tei_m, EV_REMOVE, dp);
 784         else if (mt == ID_VERIFY)
 785                 mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, dp);
 786         else if (mt == ID_CHK_RES)
 787                 mISDN_FsmEvent(&tm->tei_m, EV_CHKRESP, dp);
 788 }
 789
 790 static struct layer2 *
 791 create_new_tei(struct manager *mgr, int tei, int sapi)
 792 {
 793         unsigned long           opt = 0;
 794         unsigned long           flags;
 795         int                     id;
 796         struct layer2           *l2;
 797         struct channel_req      rq;
 798
 799         if (!mgr->up)
 800                 return NULL;
 801         if ((tei >= 0) && (tei < 64))
 802                 test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
 803         if (mgr->ch.st->dev->Dprotocols & ((1 << ISDN_P_TE_E1) |
 804             (1 << ISDN_P_NT_E1))) {
 805                 test_and_set_bit(OPTION_L2_PMX, &opt);
 806                 rq.protocol = ISDN_P_NT_E1;
 807         } else {
 808                 rq.protocol = ISDN_P_NT_S0;
 809         }
 810         l2 = create_l2(mgr->up, ISDN_P_LAPD_NT, opt, tei, sapi);
 811         if (!l2) {
 812                 printk(KERN_WARNING "%s:no memory for layer2\n", __func__);
 813                 return NULL;
 814         }
 815         l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL);
 816         if (!l2->tm) {
 817                 kfree(l2);
 818                 printk(KERN_WARNING "%s:no memory for teimgr\n", __func__);
 819                 return NULL;
 820         }
 821         l2->tm->mgr = mgr;
 822         l2->tm->l2 = l2;
 823         l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM;
 824         l2->tm->tei_m.userdata = l2->tm;
 825         l2->tm->tei_m.printdebug = tei_debug;
 826         l2->tm->tei_m.fsm = &teifsmn;
 827         l2->tm->tei_m.state = ST_TEI_NOP;
 828         l2->tm->tval = 2000; /* T202  2 sec */
 829         mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
 830         write_lock_irqsave(&mgr->lock, flags);
 831         id = get_free_id(mgr);
 832         list_add_tail(&l2->list, &mgr->layer2);
 833         write_unlock_irqrestore(&mgr->lock, flags);
 834         if (id < 0) {
 835                 l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
 836                 printk(KERN_WARNING "%s:no free id\n", __func__);
 837                 return NULL;
 838         } else {
 839                 l2->ch.nr = id;
 840                 __add_layer2(&l2->ch, mgr->ch.st);
 841                 l2->ch.recv = mgr->ch.recv;
 842                 l2->ch.peer = mgr->ch.peer;
 843                 l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
 844                 /* We need open here L1 for the manager as well (refcounting) */
 845                 rq.adr.dev = mgr->ch.st->dev->id;
 846                 id = mgr->ch.st->own.ctrl(&mgr->ch.st->own, OPEN_CHANNEL, &rq);
 847                 if (id < 0) {
 848                         printk(KERN_WARNING "%s: cannot open L1\n", __func__);
 849                         l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
 850                         l2 = NULL;
 851                 }
 852         }
 853         return l2;
 854 }
 855
 856 static void
 857 new_tei_req(struct manager *mgr, u_char *dp)
 858 {
 859         int             tei, ri;
 860         struct layer2   *l2;
 861
 862         ri = dp[0] << 8;
 863         ri += dp[1];
 864         if (!mgr->up)
 865                 goto denied;
 866         if (!(dp[3] & 1)) /* Extension bit != 1 */
 867                 goto denied;
 868         if (dp[3] != 0xff)
 869                 tei = dp[3] >> 1; /* 3GPP TS 08.56 6.1.11.2 */
 870         else
 871                 tei = get_free_tei(mgr);
 872         if (tei < 0) {
 873                 printk(KERN_WARNING "%s:No free tei\n", __func__);
 874                 goto denied;
 875         }
 876         l2 = create_new_tei(mgr, tei, CTRL_SAPI);
 877         if (!l2)
 878                 goto denied;
 879         else
 880                 mISDN_FsmEvent(&l2->tm->tei_m, EV_ASSIGN_REQ, dp);
 881         return;
 882 denied:
 883         put_tei_msg(mgr, ID_DENIED, ri, GROUP_TEI);
 884 }
 885
 886 static int
 887 ph_data_ind(struct manager *mgr, struct sk_buff *skb)
 888 {
 889         int             ret = -EINVAL;
 890         struct layer2   *l2, *nl2;
 891         u_char          mt;
 892
 893         if (skb->len < 8) {
 894                 if (*debug  & DEBUG_L2_TEI)
 895                         printk(KERN_DEBUG "%s: short mgr frame %d/8\n",
 896                                __func__, skb->len);
 897                 goto done;
 898         }
 899
 900         if ((skb->data[0] >> 2) != TEI_SAPI) /* not for us */
 901                 goto done;
 902         if (skb->data[0] & 1) /* EA0 formal error */
 903                 goto done;
 904         if (!(skb->data[1] & 1)) /* EA1 formal error */
 905                 goto done;
 906         if ((skb->data[1] >> 1) != GROUP_TEI) /* not for us */
 907                 goto done;
 908         if ((skb->data[2] & 0xef) != UI) /* not UI */
 909                 goto done;
 910         if (skb->data[3] != TEI_ENTITY_ID) /* not tei entity */
 911                 goto done;
 912         mt = skb->data[6];
 913         switch (mt) {
 914         case ID_REQUEST:
 915         case ID_CHK_RES:
 916         case ID_VERIFY:
 917                 if (!test_bit(MGR_OPT_NETWORK, &mgr->options))
 918                         goto done;
 919                 break;
 920         case ID_ASSIGNED:
 921         case ID_DENIED:
 922         case ID_CHK_REQ:
 923         case ID_REMOVE:
 924                 if (test_bit(MGR_OPT_NETWORK, &mgr->options))
 925                         goto done;
 926                 break;
 927         default:
 928                 goto done;
 929         }
 930         ret = 0;
 931         if (mt == ID_REQUEST) {
 932                 new_tei_req(mgr, &skb->data[4]);
 933                 goto done;
 934         }
 935         list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
 936                 tei_ph_data_ind(l2->tm, mt, &skb->data[4], skb->len - 4);
 937         }
 938 done:
 939         return ret;
 940 }
 941
 942 int
 943 l2_tei(struct layer2 *l2, u_int cmd, u_long arg)
 944 {
 945         struct teimgr   *tm = l2->tm;
 946
 947         if (test_bit(FLG_FIXED_TEI, &l2->flag))
 948                 return 0;
 949         if (*debug & DEBUG_L2_TEI)
 950                 printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd);
 951         switch (cmd) {
 952         case MDL_ASSIGN_IND:
 953                 mISDN_FsmEvent(&tm->tei_m, EV_IDREQ, NULL);
 954                 break;
 955         case MDL_ERROR_IND:
 956                 if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
 957                         mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, &l2->tei);
 958                 if (test_bit(MGR_OPT_USER, &tm->mgr->options))
 959                         mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, NULL);
 960                 break;
 961         case MDL_STATUS_UP_IND:
 962                 if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
 963                         mISDN_FsmEvent(&tm->mgr->deact, EV_ACTIVATE, NULL);
 964                 break;
 965         case MDL_STATUS_DOWN_IND:
 966                 if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
 967                         mISDN_FsmEvent(&tm->mgr->deact, EV_DEACTIVATE, NULL);
 968                 break;
 969         case MDL_STATUS_UI_IND:
 970                 if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
 971                         mISDN_FsmEvent(&tm->mgr->deact, EV_UI, NULL);
 972                 break;
 973         }
 974         return 0;
 975 }
 976
 977 void
 978 TEIrelease(struct layer2 *l2)
 979 {
 980         struct teimgr   *tm = l2->tm;
 981         u_long          flags;
 982
 983         mISDN_FsmDelTimer(&tm->timer, 1);
 984         write_lock_irqsave(&tm->mgr->lock, flags);
 985         list_del(&l2->list);
 986         write_unlock_irqrestore(&tm->mgr->lock, flags);
 987         l2->tm = NULL;
 988         kfree(tm);
 989 }
 990
 991 static int
 992 create_teimgr(struct manager *mgr, struct channel_req *crq)
 993 {
 994         struct layer2           *l2;
 995         unsigned long           opt = 0;
 996         unsigned long           flags;
 997         int                     id;
 998         struct channel_req      l1rq;
 999
1000         if (*debug & DEBUG_L2_TEI)
1001                 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
1002                        __func__, dev_name(&mgr->ch.st->dev->dev),
1003                        crq->protocol, crq->adr.dev, crq->adr.channel,
1004                        crq->adr.sapi, crq->adr.tei);
1005         if (crq->adr.tei > GROUP_TEI)
1006                 return -EINVAL;
1007         if (crq->adr.tei < 64)
1008                 test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
1009         if (crq->adr.tei == 0)
1010                 test_and_set_bit(OPTION_L2_PTP, &opt);
1011         if (test_bit(MGR_OPT_NETWORK, &mgr->options)) {
1012                 if (crq->protocol == ISDN_P_LAPD_TE)
1013                         return -EPROTONOSUPPORT;
1014                 if ((crq->adr.tei != 0) && (crq->adr.tei != 127))
1015                         return -EINVAL;
1016                 if (mgr->up) {
1017                         printk(KERN_WARNING
1018                                "%s: only one network manager is allowed\n",
1019                                __func__);
1020                         return -EBUSY;
1021                 }
1022         } else if (test_bit(MGR_OPT_USER, &mgr->options)) {
1023                 if (crq->protocol == ISDN_P_LAPD_NT)
1024                         return -EPROTONOSUPPORT;
1025                 if ((crq->adr.tei >= 64) && (crq->adr.tei < GROUP_TEI))
1026                         return -EINVAL; /* dyn tei */
1027         } else {
1028                 if (crq->protocol == ISDN_P_LAPD_NT)
1029                         test_and_set_bit(MGR_OPT_NETWORK, &mgr->options);
1030                 if (crq->protocol == ISDN_P_LAPD_TE)
1031                         test_and_set_bit(MGR_OPT_USER, &mgr->options);
1032         }
1033         l1rq.adr = crq->adr;
1034         if (mgr->ch.st->dev->Dprotocols
1035             & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
1036                 test_and_set_bit(OPTION_L2_PMX, &opt);
1037         if ((crq->protocol == ISDN_P_LAPD_NT) && (crq->adr.tei == 127)) {
1038                 mgr->up = crq->ch;
1039                 id = DL_INFO_L2_CONNECT;
1040                 teiup_create(mgr, DL_INFORMATION_IND, sizeof(id), &id);
1041                 if (test_bit(MGR_PH_ACTIVE, &mgr->options))
1042                         teiup_create(mgr, PH_ACTIVATE_IND, 0, NULL);
1043                 crq->ch = NULL;
1044                 if (!list_empty(&mgr->layer2)) {
1045                         read_lock_irqsave(&mgr->lock, flags);
1046                         list_for_each_entry(l2, &mgr->layer2, list) {
1047                                 l2->up = mgr->up;
1048                                 l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
1049                         }
1050                         read_unlock_irqrestore(&mgr->lock, flags);
1051                 }
1052                 return 0;
1053         }
1054         l2 = create_l2(crq->ch, crq->protocol, opt,
1055                        crq->adr.tei, crq->adr.sapi);
1056         if (!l2)
1057                 return -ENOMEM;
1058         l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL);
1059         if (!l2->tm) {
1060                 kfree(l2);
1061                 printk(KERN_ERR "kmalloc teimgr failed\n");
1062                 return -ENOMEM;
1063         }
1064         l2->tm->mgr = mgr;
1065         l2->tm->l2 = l2;
1066         l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM;
1067         l2->tm->tei_m.userdata = l2->tm;
1068         l2->tm->tei_m.printdebug = tei_debug;
1069         if (crq->protocol == ISDN_P_LAPD_TE) {
1070                 l2->tm->tei_m.fsm = &teifsmu;
1071                 l2->tm->tei_m.state = ST_TEI_NOP;
1072                 l2->tm->tval = 1000; /* T201  1 sec */
1073                 if (test_bit(OPTION_L2_PMX, &opt))
1074                         l1rq.protocol = ISDN_P_TE_E1;
1075                 else
1076                         l1rq.protocol = ISDN_P_TE_S0;
1077         } else {
1078                 l2->tm->tei_m.fsm = &teifsmn;
1079                 l2->tm->tei_m.state = ST_TEI_NOP;
1080                 l2->tm->tval = 2000; /* T202  2 sec */
1081                 if (test_bit(OPTION_L2_PMX, &opt))
1082                         l1rq.protocol = ISDN_P_NT_E1;
1083                 else
1084                         l1rq.protocol = ISDN_P_NT_S0;
1085         }
1086         mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
1087         write_lock_irqsave(&mgr->lock, flags);
1088         id = get_free_id(mgr);
1089         list_add_tail(&l2->list, &mgr->layer2);
1090         write_unlock_irqrestore(&mgr->lock, flags);
1091         if (id >= 0) {
1092                 l2->ch.nr = id;
1093                 l2->up->nr = id;
1094                 crq->ch = &l2->ch;
1095                 /* We need open here L1 for the manager as well (refcounting) */
1096                 id = mgr->ch.st->own.ctrl(&mgr->ch.st->own, OPEN_CHANNEL,
1097                                           &l1rq);
1098         }
1099         if (id < 0)
1100                 l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
1101         return id;
1102 }
1103
1104 static int
1105 mgr_send(struct mISDNchannel *ch, struct sk_buff *skb)
1106 {
1107         struct manager  *mgr;
1108         struct mISDNhead        *hh =  mISDN_HEAD_P(skb);
1109         int                     ret = -EINVAL;
1110
1111         mgr = container_of(ch, struct manager, ch);
1112         if (*debug & DEBUG_L2_RECV)
1113                 printk(KERN_DEBUG "%s: prim(%x) id(%x)\n",
1114                        __func__, hh->prim, hh->id);
1115         switch (hh->prim) {
1116         case PH_DATA_IND:
1117                 mISDN_FsmEvent(&mgr->deact, EV_UI, NULL);
1118                 ret = ph_data_ind(mgr, skb);
1119                 break;
1120         case PH_DATA_CNF:
1121                 do_ack(mgr, hh->id);
1122                 ret = 0;
1123                 break;
1124         case PH_ACTIVATE_IND:
1125                 test_and_set_bit(MGR_PH_ACTIVE, &mgr->options);
1126                 if (mgr->up)
1127                         teiup_create(mgr, PH_ACTIVATE_IND, 0, NULL);
1128                 mISDN_FsmEvent(&mgr->deact, EV_ACTIVATE_IND, NULL);
1129                 do_send(mgr);
1130                 ret = 0;
1131                 break;
1132         case PH_DEACTIVATE_IND:
1133                 test_and_clear_bit(MGR_PH_ACTIVE, &mgr->options);
1134                 if (mgr->up)
1135                         teiup_create(mgr, PH_DEACTIVATE_IND, 0, NULL);
1136                 mISDN_FsmEvent(&mgr->deact, EV_DEACTIVATE_IND, NULL);
1137                 ret = 0;
1138                 break;
1139         case DL_UNITDATA_REQ:
1140                 return dl_unit_data(mgr, skb);
1141         }
1142         if (!ret)
1143                 dev_kfree_skb(skb);
1144         return ret;
1145 }
1146
1147 static int
1148 free_teimanager(struct manager *mgr)
1149 {
1150         struct layer2   *l2, *nl2;
1151
1152         test_and_clear_bit(OPTION_L1_HOLD, &mgr->options);
1153         if (test_bit(MGR_OPT_NETWORK, &mgr->options)) {
1154                 /* not locked lock is taken in release tei */
1155                 mgr->up = NULL;
1156                 if (test_bit(OPTION_L2_CLEANUP, &mgr->options)) {
1157                         list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
1158                                 put_tei_msg(mgr, ID_REMOVE, 0, l2->tei);
1159                                 mutex_lock(&mgr->ch.st->lmutex);
1160                                 list_del(&l2->ch.list);
1161                                 mutex_unlock(&mgr->ch.st->lmutex);
1162                                 l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
1163                         }
1164                         test_and_clear_bit(MGR_OPT_NETWORK, &mgr->options);
1165                 } else {
1166                         list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
1167                                 l2->up = NULL;
1168                         }
1169                 }
1170         }
1171         if (test_bit(MGR_OPT_USER, &mgr->options)) {
1172                 if (list_empty(&mgr->layer2))
1173                         test_and_clear_bit(MGR_OPT_USER, &mgr->options);
1174         }
1175         mgr->ch.st->dev->D.ctrl(&mgr->ch.st->dev->D, CLOSE_CHANNEL, NULL);
1176         return 0;
1177 }
1178
1179 static int
1180 ctrl_teimanager(struct manager *mgr, void *arg)
1181 {
1182         /* currently we only have one option */
1183         int     *val = (int *)arg;
1184         int     ret = 0;
1185
1186         switch (val[0]) {
1187         case IMCLEAR_L2:
1188                 if (val[1])
1189                         test_and_set_bit(OPTION_L2_CLEANUP, &mgr->options);
1190                 else
1191                         test_and_clear_bit(OPTION_L2_CLEANUP, &mgr->options);
1192                 break;
1193         case IMHOLD_L1:
1194                 if (val[1])
1195                         test_and_set_bit(OPTION_L1_HOLD, &mgr->options);
1196                 else
1197                         test_and_clear_bit(OPTION_L1_HOLD, &mgr->options);
1198                 break;
1199         default:
1200                 ret = -EINVAL;
1201         }
1202         return ret;
1203 }
1204
1205 /* This function does create a L2 for fixed TEI in NT Mode */
1206 static int
1207 check_data(struct manager *mgr, struct sk_buff *skb)
1208 {
1209         struct mISDNhead        *hh =  mISDN_HEAD_P(skb);
1210         int                     ret, tei, sapi;
1211         struct layer2           *l2;
1212
1213         if (*debug & DEBUG_L2_CTRL)
1214                 printk(KERN_DEBUG "%s: prim(%x) id(%x)\n",
1215                        __func__, hh->prim, hh->id);
1216         if (test_bit(MGR_OPT_USER, &mgr->options))
1217                 return -ENOTCONN;
1218         if (hh->prim != PH_DATA_IND)
1219                 return -ENOTCONN;
1220         if (skb->len != 3)
1221                 return -ENOTCONN;
1222         if (skb->data[0] & 3) /* EA0 and CR must be  0 */
1223                 return -EINVAL;
1224         sapi = skb->data[0] >> 2;
1225         if (!(skb->data[1] & 1)) /* invalid EA1 */
1226                 return -EINVAL;
1227         tei = skb->data[1] >> 1;
1228         if (tei > 63) /* not a fixed tei */
1229                 return -ENOTCONN;
1230         if ((skb->data[2] & ~0x10) != SABME)
1231                 return -ENOTCONN;
1232         /* We got a SABME for a fixed TEI */
1233         if (*debug & DEBUG_L2_CTRL)
1234                 printk(KERN_DEBUG "%s: SABME sapi(%d) tei(%d)\n",
1235                        __func__, sapi, tei);
1236         l2 = create_new_tei(mgr, tei, sapi);
1237         if (!l2) {
1238                 if (*debug & DEBUG_L2_CTRL)
1239                         printk(KERN_DEBUG "%s: failed to create new tei\n",
1240                                __func__);
1241                 return -ENOMEM;
1242         }
1243         ret = l2->ch.send(&l2->ch, skb);
1244         return ret;
1245 }
1246
1247 void
1248 delete_teimanager(struct mISDNchannel *ch)
1249 {
1250         struct manager  *mgr;
1251         struct layer2   *l2, *nl2;
1252
1253         mgr = container_of(ch, struct manager, ch);
1254         /* not locked lock is taken in release tei */
1255         list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
1256                 mutex_lock(&mgr->ch.st->lmutex);
1257                 list_del(&l2->ch.list);
1258                 mutex_unlock(&mgr->ch.st->lmutex);
1259                 l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
1260         }
1261         list_del(&mgr->ch.list);
1262         list_del(&mgr->bcast.list);
1263         skb_queue_purge(&mgr->sendq);
1264         kfree(mgr);
1265 }
1266
1267 static int
1268 mgr_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
1269 {
1270         struct manager  *mgr;
1271         int             ret = -EINVAL;
1272
1273         mgr = container_of(ch, struct manager, ch);
1274         if (*debug & DEBUG_L2_CTRL)
1275                 printk(KERN_DEBUG "%s(%x, %p)\n", __func__, cmd, arg);
1276         switch (cmd) {
1277         case OPEN_CHANNEL:
1278                 ret = create_teimgr(mgr, arg);
1279                 break;
1280         case CLOSE_CHANNEL:
1281                 ret = free_teimanager(mgr);
1282                 break;
1283         case CONTROL_CHANNEL:
1284                 ret = ctrl_teimanager(mgr, arg);
1285                 break;
1286         case CHECK_DATA:
1287                 ret = check_data(mgr, arg);
1288                 break;
1289         }
1290         return ret;
1291 }
1292
1293 static int
1294 mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb)
1295 {
1296         struct manager          *mgr = container_of(ch, struct manager, bcast);
1297         struct mISDNhead        *hhc, *hh = mISDN_HEAD_P(skb);
1298         struct sk_buff          *cskb = NULL;
1299         struct layer2           *l2;
1300         u_long                  flags;
1301         int                     ret;
1302
1303         read_lock_irqsave(&mgr->lock, flags);
1304         list_for_each_entry(l2, &mgr->layer2, list) {
1305                 if ((hh->id & MISDN_ID_SAPI_MASK) ==
1306                     (l2->ch.addr & MISDN_ID_SAPI_MASK)) {
1307                         if (list_is_last(&l2->list, &mgr->layer2)) {
1308                                 cskb = skb;
1309                                 skb = NULL;
1310                         } else {
1311                                 if (!cskb)
1312                                         cskb = skb_copy(skb, GFP_ATOMIC);
1313                         }
1314                         if (cskb) {
1315                                 hhc = mISDN_HEAD_P(cskb);
1316                                 /* save original header behind normal header */
1317                                 hhc++;
1318                                 *hhc = *hh;
1319                                 hhc--;
1320                                 hhc->prim = DL_INTERN_MSG;
1321                                 hhc->id = l2->ch.nr;
1322                                 ret = ch->st->own.recv(&ch->st->own, cskb);
1323                                 if (ret) {
1324                                         if (*debug & DEBUG_SEND_ERR)
1325                                                 printk(KERN_DEBUG
1326                                                        "%s ch%d prim(%x) addr(%x)"
1327                                                        " err %d\n",
1328                                                        __func__, l2->ch.nr,
1329                                                        hh->prim, l2->ch.addr, ret);
1330                                 } else
1331                                         cskb = NULL;
1332                         } else {
1333                                 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
1334                                        __func__, ch->nr, ch->addr);
1335                                 goto out;
1336                         }
1337                 }
1338         }
1339 out:
1340         read_unlock_irqrestore(&mgr->lock, flags);
1341         if (cskb)
1342                 dev_kfree_skb(cskb);
1343         if (skb)
1344                 dev_kfree_skb(skb);
1345         return 0;
1346 }
1347
1348 static int
1349 mgr_bcast_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
1350 {
1351
1352         return -EINVAL;
1353 }
1354
1355 int
1356 create_teimanager(struct mISDNdevice *dev)
1357 {
1358         struct manager *mgr;
1359
1360         mgr = kzalloc(sizeof(struct manager), GFP_KERNEL);
1361         if (!mgr)
1362                 return -ENOMEM;
1363         INIT_LIST_HEAD(&mgr->layer2);
1364         rwlock_init(&mgr->lock);
1365         skb_queue_head_init(&mgr->sendq);
1366         mgr->nextid = 1;
1367         mgr->lastid = MISDN_ID_NONE;
1368         mgr->ch.send = mgr_send;
1369         mgr->ch.ctrl = mgr_ctrl;
1370         mgr->ch.st = dev->D.st;
1371         set_channel_address(&mgr->ch, TEI_SAPI, GROUP_TEI);
1372         add_layer2(&mgr->ch, dev->D.st);
1373         mgr->bcast.send = mgr_bcast;
1374         mgr->bcast.ctrl = mgr_bcast_ctrl;
1375         mgr->bcast.st = dev->D.st;
1376         set_channel_address(&mgr->bcast, 0, GROUP_TEI);
1377         add_layer2(&mgr->bcast, dev->D.st);
1378         mgr->deact.debug = *debug & DEBUG_MANAGER;
1379         mgr->deact.userdata = mgr;
1380         mgr->deact.printdebug = da_debug;
1381         mgr->deact.fsm = &deactfsm;
1382         mgr->deact.state = ST_L1_DEACT;
1383         mISDN_FsmInitTimer(&mgr->deact, &mgr->datimer);
1384         dev->teimgr = &mgr->ch;
1385         return 0;
1386 }
1387
1388 int TEIInit(u_int *deb)
1389 {
1390         int res;
1391         debug = deb;
1392         teifsmu.state_count = TEI_STATE_COUNT;
1393         teifsmu.event_count = TEI_EVENT_COUNT;
1394         teifsmu.strEvent = strTeiEvent;
1395         teifsmu.strState = strTeiState;
1396         res = mISDN_FsmNew(&teifsmu, TeiFnListUser, ARRAY_SIZE(TeiFnListUser));
1397         if (res)
1398                 goto error;
1399         teifsmn.state_count = TEI_STATE_COUNT;
1400         teifsmn.event_count = TEI_EVENT_COUNT;
1401         teifsmn.strEvent = strTeiEvent;
1402         teifsmn.strState = strTeiState;
1403         res = mISDN_FsmNew(&teifsmn, TeiFnListNet, ARRAY_SIZE(TeiFnListNet));
1404         if (res)
1405                 goto error_smn;
1406         deactfsm.state_count =  DEACT_STATE_COUNT;
1407         deactfsm.event_count = DEACT_EVENT_COUNT;
1408         deactfsm.strEvent = strDeactEvent;
1409         deactfsm.strState = strDeactState;
1410         res = mISDN_FsmNew(&deactfsm, DeactFnList, ARRAY_SIZE(DeactFnList));
1411         if (res)
1412                 goto error_deact;
1413         return 0;
1414
1415 error_deact:
1416         mISDN_FsmFree(&teifsmn);
1417 error_smn:
1418         mISDN_FsmFree(&teifsmu);
1419 error:
1420         return res;
1421 }
1422
1423 void TEIFree(void)
1424 {
1425         mISDN_FsmFree(&teifsmu);
1426         mISDN_FsmFree(&teifsmn);
1427         mISDN_FsmFree(&deactfsm);
1428 }