tei.c

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