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