linux/drivers/isdn/mISDN/tei.c

/*
 *
 * Author	Karsten Keil <kkeil@novell.com>
 *
 * Copyright 2008  by Karsten Keil <kkeil@novell.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */
#include "layer2.h"
#include <linux/random.h>
#include "core.h"

#define ID_REQUEST	1
#define ID_ASSIGNED	2
#define ID_DENIED	3
#define ID_CHK_REQ	4
#define ID_CHK_RES	5
#define ID_REMOVE	6
#define ID_VERIFY	7

#define TEI_ENTITY_ID	0xf

#define MGR_PH_ACTIVE	16
#define MGR_PH_NOTREADY	17

#define DATIMER_VAL	10000

static 	u_int	*debug;

static struct Fsm deactfsm = {NULL, 0, 0, NULL, NULL};
static struct Fsm teifsmu = {NULL, 0, 0, NULL, NULL};
static struct Fsm teifsmn = {NULL, 0, 0, NULL, NULL};

enum {
	ST_L1_DEACT,
	ST_L1_DEACT_PENDING,
	ST_L1_ACTIV,
};
#define DEACT_STATE_COUNT (ST_L1_ACTIV+1)

static char *strDeactState[] =
{
	"ST_L1_DEACT",
	"ST_L1_DEACT_PENDING",
	"ST_L1_ACTIV",
};

enum {
	EV_ACTIVATE,
	EV_ACTIVATE_IND,
	EV_DEACTIVATE,
	EV_DEACTIVATE_IND,
	EV_UI,
	EV_DATIMER,
};

#define DEACT_EVENT_COUNT (EV_DATIMER+1)

static char *strDeactEvent[] =
{
	"EV_ACTIVATE",
	"EV_ACTIVATE_IND",
	"EV_DEACTIVATE",
	"EV_DEACTIVATE_IND",
	"EV_UI",
	"EV_DATIMER",
};

static void
da_debug(struct FsmInst *fi, char *fmt, ...)
{
	struct manager	*mgr = fi->userdata;
	va_list va;

	if (!(*debug & DEBUG_L2_TEIFSM))
		return;
	va_start(va, fmt);
	printk(KERN_DEBUG "mgr(%d): ", mgr->ch.st->dev->id);
	vprintk(fmt, va);
	printk("\n");
	va_end(va);
}

static void
da_activate(struct FsmInst *fi, int event, void *arg)
{
	struct manager	*mgr = fi->userdata;

	if (fi->state == ST_L1_DEACT_PENDING)
		mISDN_FsmDelTimer(&mgr->datimer, 1);
	mISDN_FsmChangeState(fi, ST_L1_ACTIV);
}

static void
da_deactivate_ind(struct FsmInst *fi, int event, void *arg)
{
	mISDN_FsmChangeState(fi, ST_L1_DEACT);
}

static void
da_deactivate(struct FsmInst *fi, int event, void *arg)
{
	struct manager	*mgr = fi->userdata;
	struct layer2	*l2;
	u_long		flags;

	read_lock_irqsave(&mgr->lock, flags);
	list_for_each_entry(l2, &mgr->layer2, list) {
		if (l2->l2m.state > ST_L2_4) {
			/* have still activ TEI */
			read_unlock_irqrestore(&mgr->lock, flags);
			return;
		}
	}
	read_unlock_irqrestore(&mgr->lock, flags);
	/* All TEI are inactiv */
	if (!test_bit(OPTION_L1_HOLD, &mgr->options)) {
		mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER,
			NULL, 1);
		mISDN_FsmChangeState(fi, ST_L1_DEACT_PENDING);
	}
}

static void
da_ui(struct FsmInst *fi, int event, void *arg)
{
	struct manager	*mgr = fi->userdata;

	/* restart da timer */
	if (!test_bit(OPTION_L1_HOLD, &mgr->options)) {
		mISDN_FsmDelTimer(&mgr->datimer, 2);
		mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER,
			NULL, 2);
	}
}

static void
da_timer(struct FsmInst *fi, int event, void *arg)
{
	struct manager	*mgr = fi->userdata;
	struct layer2	*l2;
	u_long		flags;

	/* check again */
	read_lock_irqsave(&mgr->lock, flags);
	list_for_each_entry(l2, &mgr->layer2, list) {
		if (l2->l2m.state > ST_L2_4) {
			/* have still activ TEI */
			read_unlock_irqrestore(&mgr->lock, flags);
			mISDN_FsmChangeState(fi, ST_L1_ACTIV);
			return;
		}
	}
	read_unlock_irqrestore(&mgr->lock, flags);
	/* All TEI are inactiv */
	mISDN_FsmChangeState(fi, ST_L1_DEACT);
	_queue_data(&mgr->ch, PH_DEACTIVATE_REQ, MISDN_ID_ANY, 0, NULL,
	    GFP_ATOMIC);
}

static struct FsmNode DeactFnList[] =
{
	{ST_L1_DEACT, EV_ACTIVATE_IND, da_activate},
	{ST_L1_ACTIV, EV_DEACTIVATE_IND, da_deactivate_ind},
	{ST_L1_ACTIV, EV_DEACTIVATE, da_deactivate},
	{ST_L1_DEACT_PENDING, EV_ACTIVATE, da_activate},
	{ST_L1_DEACT_PENDING, EV_UI, da_ui},
	{ST_L1_DEACT_PENDING, EV_DATIMER, da_timer},
};

enum {
	ST_TEI_NOP,
	ST_TEI_IDREQ,
	ST_TEI_IDVERIFY,
};

#define TEI_STATE_COUNT (ST_TEI_IDVERIFY+1)

static char *strTeiState[] =
{
	"ST_TEI_NOP",
	"ST_TEI_IDREQ",
	"ST_TEI_IDVERIFY",
};

enum {
	EV_IDREQ,
	EV_ASSIGN,
	EV_ASSIGN_REQ,
	EV_DENIED,
	EV_CHKREQ,
	EV_CHKRESP,
	EV_REMOVE,
	EV_VERIFY,
	EV_TIMER,
};

#define TEI_EVENT_COUNT (EV_TIMER+1)

static char *strTeiEvent[] =
{
	"EV_IDREQ",
	"EV_ASSIGN",
	"EV_ASSIGN_REQ",
	"EV_DENIED",
	"EV_CHKREQ",
	"EV_CHKRESP",
	"EV_REMOVE",
	"EV_VERIFY",
	"EV_TIMER",
};

static void
tei_debug(struct FsmInst *fi, char *fmt, ...)
{
	struct teimgr	*tm = fi->userdata;
	va_list va;

	if (!(*debug & DEBUG_L2_TEIFSM))
		return;
	va_start(va, fmt);
	printk(KERN_DEBUG "tei(%d): ", tm->l2->tei);
	vprintk(fmt, va);
	printk("\n");
	va_end(va);
}



static int
get_free_id(struct manager *mgr)
{
	u64		ids = 0;
	int		i;
	struct layer2	*l2;

	list_for_each_entry(l2, &mgr->layer2, list) {
		if (l2->ch.nr > 63) {
			printk(KERN_WARNING
			    "%s: more as 63 layer2 for one device\n",
			    __func__);
			return -EBUSY;
		}
		test_and_set_bit(l2->ch.nr, (u_long *)&ids);
	}
	for (i = 1; i < 64; i++)
		if (!test_bit(i, (u_long *)&ids))
			return i;
	printk(KERN_WARNING "%s: more as 63 layer2 for one device\n",
	    __func__);
	return -EBUSY;
}

static int
get_free_tei(struct manager *mgr)
{
	u64		ids = 0;
	int		i;
	struct layer2	*l2;

	list_for_each_entry(l2, &mgr->layer2, list) {
		if (l2->ch.nr == 0)
			continue;
		if ((l2->ch.addr & 0xff) != 0)
			continue;
		i = l2->ch.addr >> 8;
		if (i < 64)
			continue;
		i -= 64;

		test_and_set_bit(i, (u_long *)&ids);
	}
	for (i = 0; i < 64; i++)
		if (!test_bit(i, (u_long *)&ids))
			return i + 64;
	printk(KERN_WARNING "%s: more as 63 dynamic tei for one device\n",
	    __func__);
	return -1;
}

static void
teiup_create(struct manager *mgr, u_int prim, int len, void *arg)
{
	struct sk_buff	*skb;
	struct mISDNhead *hh;
	int		err;

	skb = mI_alloc_skb(len, GFP_ATOMIC);
	if (!skb)
		return;
	hh = mISDN_HEAD_P(skb);
	hh->prim = prim;
	hh->id = (mgr->ch.nr << 16) | mgr->ch.addr;
	if (len)
		memcpy(skb_put(skb, len), arg, len);
	err = mgr->up->send(mgr->up, skb);
	if (err) {
		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
		dev_kfree_skb(skb);
	}
}

static u_int
new_id(struct manager *mgr)
{
	u_int	id;

	id = mgr->nextid++;
	if (id == 0x7fff)
		mgr->nextid = 1;
	id <<= 16;
	id |= GROUP_TEI << 8;
	id |= TEI_SAPI;
	return id;
}

static void
do_send(struct manager *mgr)
{
	if (!test_bit(MGR_PH_ACTIVE, &mgr->options))
		return;

	if (!test_and_set_bit(MGR_PH_NOTREADY, &mgr->options)) {
		struct sk_buff	*skb = skb_dequeue(&mgr->sendq);

		if (!skb) {
			test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
			return;
		}
		mgr->lastid = mISDN_HEAD_ID(skb);
		mISDN_FsmEvent(&mgr->deact, EV_UI, NULL);
		if (mgr->ch.recv(mgr->ch.peer, skb)) {
			dev_kfree_skb(skb);
			test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
			mgr->lastid = MISDN_ID_NONE;
		}
	}
}

static void
do_ack(struct manager *mgr, u_int id)
{
	if (test_bit(MGR_PH_NOTREADY, &mgr->options)) {
		if (id == mgr->lastid) {
			if (test_bit(MGR_PH_ACTIVE, &mgr->options)) {
				struct sk_buff	*skb;

				skb = skb_dequeue(&mgr->sendq);
				if (skb) {
					mgr->lastid = mISDN_HEAD_ID(skb);
					if (!mgr->ch.recv(mgr->ch.peer, skb))
						return;
					dev_kfree_skb(skb);
				}
			}
			mgr->lastid = MISDN_ID_NONE;
			test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options);
		}
	}
}

static void
mgr_send_down(struct manager *mgr, struct sk_buff *skb)
{
	skb_queue_tail(&mgr->sendq, skb);
	if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) {
		_queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0,
		    NULL, GFP_KERNEL);
	} else {
		do_send(mgr);
	}
}

static int
dl_unit_data(struct manager *mgr, struct sk_buff *skb)
{
	if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) /* only net send UI */
		return -EINVAL;
	if (!test_bit(MGR_PH_ACTIVE, &mgr->options))
		_queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0,
		    NULL, GFP_KERNEL);
	skb_push(skb, 3);
	skb->data[0] = 0x02; /* SAPI 0 C/R = 1 */
	skb->data[1] = 0xff; /* TEI 127 */
	skb->data[2] = UI;   /* UI frame */
	mISDN_HEAD_PRIM(skb) = PH_DATA_REQ;
	mISDN_HEAD_ID(skb) = new_id(mgr);
	skb_queue_tail(&mgr->sendq, skb);
	do_send(mgr);
	return 0;
}

static unsigned int
random_ri(void)
{
	u16 x;

	get_random_bytes(&x, sizeof(x));
	return x;
}

static struct layer2 *
findtei(struct manager *mgr, int tei)
{
	struct layer2	*l2;
	u_long		flags;

	read_lock_irqsave(&mgr->lock, flags);
	list_for_each_entry(l2, &mgr->layer2, list) {
		if ((l2->sapi == 0) && (l2->tei > 0) &&
		    (l2->tei != GROUP_TEI) && (l2->tei == tei))
			goto done;
	}
	l2 = NULL;
done:
	read_unlock_irqrestore(&mgr->lock, flags);
	return l2;
}

static void
put_tei_msg(struct manager *mgr, u_char m_id, unsigned int ri, int tei)
{
	struct sk_buff *skb;
	u_char bp[8];

	bp[0] = (TEI_SAPI << 2);
	if (test_bit(MGR_OPT_NETWORK, &mgr->options))
		bp[0] |= 2; /* CR:=1 for net command */
	bp[1] = (GROUP_TEI << 1) | 0x1;
	bp[2] = UI;
	bp[3] = TEI_ENTITY_ID;
	bp[4] = ri >> 8;
	bp[5] = ri & 0xff;
	bp[6] = m_id;
	bp[7] = ((tei << 1) & 0xff) | 1;
	skb = _alloc_mISDN_skb(PH_DATA_REQ, new_id(mgr), 8, bp, GFP_ATOMIC);
	if (!skb) {
		printk(KERN_WARNING "%s: no skb for tei msg\n", __func__);
		return;
	}
	mgr_send_down(mgr, skb);
}

static void
tei_id_request(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;

	if (tm->l2->tei != GROUP_TEI) {
		tm->tei_m.printdebug(&tm->tei_m,
			"assign request for allready assigned tei %d",
			tm->l2->tei);
		return;
	}
	tm->ri = random_ri();
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(&tm->tei_m,
			"assign request ri %d", tm->ri);
	put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI);
	mISDN_FsmChangeState(fi, ST_TEI_IDREQ);
	mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 1);
	tm->nval = 3;
}

static void
tei_id_assign(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr	*tm = fi->userdata;
	struct layer2	*l2;
	u_char *dp = arg;
	int ri, tei;

	ri = ((unsigned int) *dp++ << 8);
	ri += *dp++;
	dp++;
	tei = *dp >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "identity assign ri %d tei %d",
			ri, tei);
	l2 = findtei(tm->mgr, tei);
	if (l2) {	/* same tei is in use */
		if (ri != l2->tm->ri) {
			tm->tei_m.printdebug(fi,
				"possible duplicate assignment tei %d", tei);
			tei_l2(l2, MDL_ERROR_RSP, 0);
		}
	} else if (ri == tm->ri) {
		mISDN_FsmDelTimer(&tm->timer, 1);
		mISDN_FsmChangeState(fi, ST_TEI_NOP);
		tei_l2(tm->l2, MDL_ASSIGN_REQ, tei);
	}
}

static void
tei_id_test_dup(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr	*tm = fi->userdata;
	struct layer2	*l2;
	u_char *dp = arg;
	int tei, ri;

	ri = ((unsigned int) *dp++ << 8);
	ri += *dp++;
	dp++;
	tei = *dp >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "foreign identity assign ri %d tei %d",
			ri, tei);
	l2 = findtei(tm->mgr, tei);
	if (l2) {	/* same tei is in use */
		if (ri != l2->tm->ri) {	/* and it wasn't our request */
			tm->tei_m.printdebug(fi,
				"possible duplicate assignment tei %d", tei);
			mISDN_FsmEvent(&l2->tm->tei_m, EV_VERIFY, NULL);
		}
	}
}

static void
tei_id_denied(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;
	u_char *dp = arg;
	int ri, tei;

	ri = ((unsigned int) *dp++ << 8);
	ri += *dp++;
	dp++;
	tei = *dp >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "identity denied ri %d tei %d",
			ri, tei);
}

static void
tei_id_chk_req(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;
	u_char *dp = arg;
	int tei;

	tei = *(dp+3) >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "identity check req tei %d", tei);
	if ((tm->l2->tei != GROUP_TEI) && ((tei == GROUP_TEI) ||
	    (tei == tm->l2->tei))) {
		mISDN_FsmDelTimer(&tm->timer, 4);
		mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP);
		put_tei_msg(tm->mgr, ID_CHK_RES, random_ri(), tm->l2->tei);
	}
}

static void
tei_id_remove(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;
	u_char *dp = arg;
	int tei;

	tei = *(dp+3) >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "identity remove tei %d", tei);
	if ((tm->l2->tei != GROUP_TEI) &&
	    ((tei == GROUP_TEI) || (tei == tm->l2->tei))) {
		mISDN_FsmDelTimer(&tm->timer, 5);
		mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP);
		tei_l2(tm->l2, MDL_REMOVE_REQ, 0);
	}
}

static void
tei_id_verify(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;

	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "id verify request for tei %d",
			tm->l2->tei);
	put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei);
	mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY);
	mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2);
	tm->nval = 2;
}

static void
tei_id_req_tout(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;

	if (--tm->nval) {
		tm->ri = random_ri();
		if (*debug & DEBUG_L2_TEI)
			tm->tei_m.printdebug(fi, "assign req(%d) ri %d",
				4 - tm->nval, tm->ri);
		put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI);
		mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 3);
	} else {
		tm->tei_m.printdebug(fi, "assign req failed");
		tei_l2(tm->l2, MDL_ERROR_RSP, 0);
		mISDN_FsmChangeState(fi, ST_TEI_NOP);
	}
}

static void
tei_id_ver_tout(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;

	if (--tm->nval) {
		if (*debug & DEBUG_L2_TEI)
			tm->tei_m.printdebug(fi,
				"id verify req(%d) for tei %d",
				3 - tm->nval, tm->l2->tei);
		put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei);
		mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4);
	} else {
		tm->tei_m.printdebug(fi, "verify req for tei %d failed",
			tm->l2->tei);
		tei_l2(tm->l2, MDL_REMOVE_REQ, 0);
		mISDN_FsmChangeState(fi, ST_TEI_NOP);
	}
}

static struct FsmNode TeiFnListUser[] =
{
	{ST_TEI_NOP, EV_IDREQ, tei_id_request},
	{ST_TEI_NOP, EV_ASSIGN, tei_id_test_dup},
	{ST_TEI_NOP, EV_VERIFY, tei_id_verify},
	{ST_TEI_NOP, EV_REMOVE, tei_id_remove},
	{ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req},
	{ST_TEI_IDREQ, EV_TIMER, tei_id_req_tout},
	{ST_TEI_IDREQ, EV_ASSIGN, tei_id_assign},
	{ST_TEI_IDREQ, EV_DENIED, tei_id_denied},
	{ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout},
	{ST_TEI_IDVERIFY, EV_REMOVE, tei_id_remove},
	{ST_TEI_IDVERIFY, EV_CHKREQ, tei_id_chk_req},
};

static void
tei_l2remove(struct layer2 *l2)
{
	put_tei_msg(l2->tm->mgr, ID_REMOVE, 0, l2->tei);
	tei_l2(l2, MDL_REMOVE_REQ, 0);
	list_del(&l2->ch.list);
	l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
}

static void
tei_assign_req(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;
	u_char *dp = arg;

	if (tm->l2->tei == GROUP_TEI) {
		tm->tei_m.printdebug(&tm->tei_m,
			"net tei assign request without tei");
		return;
	}
	tm->ri = ((unsigned int) *dp++ << 8);
	tm->ri += *dp++;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(&tm->tei_m,
			"net assign request ri %d teim %d", tm->ri, *dp);
	put_tei_msg(tm->mgr, ID_ASSIGNED, tm->ri, tm->l2->tei);
	mISDN_FsmChangeState(fi, ST_TEI_NOP);
}

static void
tei_id_chk_req_net(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr	*tm = fi->userdata;

	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "id check request for tei %d",
		    tm->l2->tei);
	tm->rcnt = 0;
	put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei);
	mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY);
	mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2);
	tm->nval = 2;
}

static void
tei_id_chk_resp(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;
	u_char *dp = arg;
	int tei;

	tei = dp[3] >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "identity check resp tei %d", tei);
	if (tei == tm->l2->tei)
		tm->rcnt++;
}

static void
tei_id_verify_net(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;
	u_char *dp = arg;
	int tei;

	tei = dp[3] >> 1;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(fi, "identity verify req tei %d/%d",
		    tei, tm->l2->tei);
	if (tei == tm->l2->tei)
		tei_id_chk_req_net(fi, event, arg);
}

static void
tei_id_ver_tout_net(struct FsmInst *fi, int event, void *arg)
{
	struct teimgr *tm = fi->userdata;

	if (tm->rcnt == 1) {
		if (*debug & DEBUG_L2_TEI)
			tm->tei_m.printdebug(fi,
			    "check req for tei %d sucessful\n", tm->l2->tei);
		mISDN_FsmChangeState(fi, ST_TEI_NOP);
	} else if (tm->rcnt > 1) {
		/* duplicate assignment; remove */
		tei_l2remove(tm->l2);
	} else if (--tm->nval) {
		if (*debug & DEBUG_L2_TEI)
			tm->tei_m.printdebug(fi,
				"id check req(%d) for tei %d",
				3 - tm->nval, tm->l2->tei);
		put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei);
		mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4);
	} else {
		tm->tei_m.printdebug(fi, "check req for tei %d failed",
			tm->l2->tei);
		mISDN_FsmChangeState(fi, ST_TEI_NOP);
		tei_l2remove(tm->l2);
	}
}

static struct FsmNode TeiFnListNet[] =
{
	{ST_TEI_NOP, EV_ASSIGN_REQ, tei_assign_req},
	{ST_TEI_NOP, EV_VERIFY, tei_id_verify_net},
	{ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req_net},
	{ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout_net},
	{ST_TEI_IDVERIFY, EV_CHKRESP, tei_id_chk_resp},
};

static void
tei_ph_data_ind(struct teimgr *tm, u_int mt, u_char *dp, int len)
{
	if (test_bit(FLG_FIXED_TEI, &tm->l2->flag))
		return;
	if (*debug & DEBUG_L2_TEI)
		tm->tei_m.printdebug(&tm->tei_m, "tei handler mt %x", mt);
	if (mt == ID_ASSIGNED)
		mISDN_FsmEvent(&tm->tei_m, EV_ASSIGN, dp);
	else if (mt == ID_DENIED)
		mISDN_FsmEvent(&tm->tei_m, EV_DENIED, dp);
	else if (mt == ID_CHK_REQ)
		mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, dp);
	else if (mt == ID_REMOVE)
		mISDN_FsmEvent(&tm->tei_m, EV_REMOVE, dp);
	else if (mt == ID_VERIFY)
		mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, dp);
	else if (mt == ID_CHK_RES)
		mISDN_FsmEvent(&tm->tei_m, EV_CHKRESP, dp);
}

static struct layer2 *
create_new_tei(struct manager *mgr, int tei, int sapi)
{
	u_long		opt = 0;
	u_long		flags;
	int		id;
	struct layer2	*l2;

	if (!mgr->up)
		return NULL;
	if ((tei >= 0) && (tei < 64))
		test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
	if (mgr->ch.st->dev->Dprotocols
	  & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
		test_and_set_bit(OPTION_L2_PMX, &opt);
	l2 = create_l2(mgr->up, ISDN_P_LAPD_NT, opt, tei, sapi);
	if (!l2) {
		printk(KERN_WARNING "%s:no memory for layer2\n", __func__);
		return NULL;
	}
	l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL);
	if (!l2->tm) {
		kfree(l2);
		printk(KERN_WARNING "%s:no memory for teimgr\n", __func__);
		return NULL;
	}
	l2->tm->mgr = mgr;
	l2->tm->l2 = l2;
	l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM;
	l2->tm->tei_m.userdata = l2->tm;
	l2->tm->tei_m.printdebug = tei_debug;
	l2->tm->tei_m.fsm = &teifsmn;
	l2->tm->tei_m.state = ST_TEI_NOP;
	l2->tm->tval = 2000; /* T202  2 sec */
	mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
	write_lock_irqsave(&mgr->lock, flags);
	id = get_free_id(mgr);
	list_add_tail(&l2->list, &mgr->layer2);
	write_unlock_irqrestore(&mgr->lock, flags);
	if (id < 0) {
		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
		printk(KERN_WARNING "%s:no free id\n", __func__);
		return NULL;
	} else {
		l2->ch.nr = id;
		__add_layer2(&l2->ch, mgr->ch.st);
		l2->ch.recv = mgr->ch.recv;
		l2->ch.peer = mgr->ch.peer;
		l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
	}
	return l2;
}

static void
new_tei_req(struct manager *mgr, u_char *dp)
{
	int		tei, ri;
	struct layer2	*l2;

	ri = dp[0] << 8;
	ri += dp[1];
	if (!mgr->up)
		goto denied;
	if (!(dp[3] & 1)) /* Extension bit != 1 */
		goto denied;
	if (dp[3] != 0xff)
		tei = dp[3] >> 1; /* 3GPP TS 08.56 6.1.11.2 */
	else
		tei = get_free_tei(mgr);
	if (tei < 0) {
		printk(KERN_WARNING "%s:No free tei\n", __func__);
		goto denied;
	}
	l2 = create_new_tei(mgr, tei, CTRL_SAPI);
	if (!l2)
		goto denied;
	else
		mISDN_FsmEvent(&l2->tm->tei_m, EV_ASSIGN_REQ, dp);
	return;
denied:
	put_tei_msg(mgr, ID_DENIED, ri, GROUP_TEI);
}

static int
ph_data_ind(struct manager *mgr, struct sk_buff *skb)
{
	int		ret = -EINVAL;
	struct layer2	*l2;
	u_long		flags;
	u_char		mt;

	if (skb->len < 8) {
		if (*debug  & DEBUG_L2_TEI)
			printk(KERN_DEBUG "%s: short mgr frame %d/8\n",
			    __func__, skb->len);
		goto done;
	}
	if (*debug  & DEBUG_L2_TEI)

	if ((skb->data[0] >> 2) != TEI_SAPI) /* not for us */
		goto done;
	if (skb->data[0] & 1) /* EA0 formal error */
		goto done;
	if (!(skb->data[1] & 1)) /* EA1 formal error */
		goto done;
	if ((skb->data[1] >> 1) != GROUP_TEI) /* not for us */
		goto done;
	if ((skb->data[2] & 0xef) != UI) /* not UI */
		goto done;
	if (skb->data[3] != TEI_ENTITY_ID) /* not tei entity */
		goto done;
	mt = skb->data[6];
	switch (mt) {
	case ID_REQUEST:
	case ID_CHK_RES:
	case ID_VERIFY:
		if (!test_bit(MGR_OPT_NETWORK, &mgr->options))
			goto done;
		break;
	case ID_ASSIGNED:
	case ID_DENIED:
	case ID_CHK_REQ:
	case ID_REMOVE:
		if (test_bit(MGR_OPT_NETWORK, &mgr->options))
			goto done;
		break;
	default:
		goto done;
	}
	ret = 0;
	if (mt == ID_REQUEST) {
		new_tei_req(mgr, &skb->data[4]);
		goto done;
	}
	read_lock_irqsave(&mgr->lock, flags);
	list_for_each_entry(l2, &mgr->layer2, list) {
		tei_ph_data_ind(l2->tm, mt, &skb->data[4], skb->len - 4);
	}
	read_unlock_irqrestore(&mgr->lock, flags);
done:
	return ret;
}

int
l2_tei(struct layer2 *l2, u_int cmd, u_long arg)
{
	struct teimgr	*tm = l2->tm;

	if (test_bit(FLG_FIXED_TEI, &l2->flag))
		return 0;
	if (*debug & DEBUG_L2_TEI)
		printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd);
	switch (cmd) {
	case MDL_ASSIGN_IND:
		mISDN_FsmEvent(&tm->tei_m, EV_IDREQ, NULL);
		break;
	case MDL_ERROR_IND:
		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
			mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, &l2->tei);
		if (test_bit(MGR_OPT_USER, &tm->mgr->options))
			mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, NULL);
		break;
	case MDL_STATUS_UP_IND:
		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
			mISDN_FsmEvent(&tm->mgr->deact, EV_ACTIVATE, NULL);
		break;
	case MDL_STATUS_DOWN_IND:
		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
			mISDN_FsmEvent(&tm->mgr->deact, EV_DEACTIVATE, NULL);
		break;
	case MDL_STATUS_UI_IND:
		if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options))
			mISDN_FsmEvent(&tm->mgr->deact, EV_UI, NULL);
		break;
	}
	return 0;
}

void
TEIrelease(struct layer2 *l2)
{
	struct teimgr	*tm = l2->tm;
	u_long		flags;

	mISDN_FsmDelTimer(&tm->timer, 1);
	write_lock_irqsave(&tm->mgr->lock, flags);
	list_del(&l2->list);
	write_unlock_irqrestore(&tm->mgr->lock, flags);
	l2->tm = NULL;
	kfree(tm);
}

static int
create_teimgr(struct manager *mgr, struct channel_req *crq)
{
	struct layer2	*l2;
	u_long 		opt = 0;
	u_long		flags;
	int		id;

	if (*debug & DEBUG_L2_TEI)
		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
			__func__, dev_name(&mgr->ch.st->dev->dev),
			crq->protocol, crq->adr.dev, crq->adr.channel,
			crq->adr.sapi, crq->adr.tei);
	if (crq->adr.tei > GROUP_TEI)
		return -EINVAL;
	if (crq->adr.tei < 64)
		test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
	if (crq->adr.tei == 0)
		test_and_set_bit(OPTION_L2_PTP, &opt);
	if (test_bit(MGR_OPT_NETWORK, &mgr->options)) {
		if (crq->protocol == ISDN_P_LAPD_TE)
			return -EPROTONOSUPPORT;
		if ((crq->adr.tei != 0) && (crq->adr.tei != 127))
			return -EINVAL;
		if (mgr->up) {
			printk(KERN_WARNING
			    "%s: only one network manager is allowed\n",
			    __func__);
			return -EBUSY;
		}
	} else if (test_bit(MGR_OPT_USER, &mgr->options)) {
		if (crq->protocol == ISDN_P_LAPD_NT)
			return -EPROTONOSUPPORT;
		if ((crq->adr.tei >= 64) && (crq->adr.tei < GROUP_TEI))
			return -EINVAL; /* dyn tei */
	} else {
		if (crq->protocol == ISDN_P_LAPD_NT)
			test_and_set_bit(MGR_OPT_NETWORK, &mgr->options);
		if (crq->protocol == ISDN_P_LAPD_TE)
			test_and_set_bit(MGR_OPT_USER, &mgr->options);
	}
	if (mgr->ch.st->dev->Dprotocols
	  & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
		test_and_set_bit(OPTION_L2_PMX, &opt);
	if ((crq->protocol == ISDN_P_LAPD_NT) && (crq->adr.tei == 127)) {
		mgr->up = crq->ch;
		id = DL_INFO_L2_CONNECT;
		teiup_create(mgr, DL_INFORMATION_IND, sizeof(id), &id);
		crq->ch = NULL;
		if (!list_empty(&mgr->layer2)) {
			read_lock_irqsave(&mgr->lock, flags);
			list_for_each_entry(l2, &mgr->layer2, list) {
				l2->up = mgr->up;
				l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
			}
			read_unlock_irqrestore(&mgr->lock, flags);
		}
		return 0;
	}
	l2 = create_l2(crq->ch, crq->protocol, opt,
		crq->adr.tei, crq->adr.sapi);
	if (!l2)
		return -ENOMEM;
	l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL);
	if (!l2->tm) {
		kfree(l2);
		printk(KERN_ERR "kmalloc teimgr failed\n");
		return -ENOMEM;
	}
	l2->tm->mgr = mgr;
	l2->tm->l2 = l2;
	l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM;
	l2->tm->tei_m.userdata = l2->tm;
	l2->tm->tei_m.printdebug = tei_debug;
	if (crq->protocol == ISDN_P_LAPD_TE) {
		l2->tm->tei_m.fsm = &teifsmu;
		l2->tm->tei_m.state = ST_TEI_NOP;
		l2->tm->tval = 1000; /* T201  1 sec */
	} else {
		l2->tm->tei_m.fsm = &teifsmn;
		l2->tm->tei_m.state = ST_TEI_NOP;
		l2->tm->tval = 2000; /* T202  2 sec */
	}
	mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
	write_lock_irqsave(&mgr->lock, flags);
	id = get_free_id(mgr);
	list_add_tail(&l2->list, &mgr->layer2);
	write_unlock_irqrestore(&mgr->lock, flags);
	if (id < 0) {
		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
	} else {
		l2->ch.nr = id;
		l2->up->nr = id;
		crq->ch = &l2->ch;
		id = 0;
	}
	return id;
}

static int
mgr_send(struct mISDNchannel *ch, struct sk_buff *skb)
{
	struct manager	*mgr;
	struct mISDNhead	*hh =  mISDN_HEAD_P(skb);
	int			ret = -EINVAL;

	mgr = container_of(ch, struct manager, ch);
	if (*debug & DEBUG_L2_RECV)
		printk(KERN_DEBUG "%s: prim(%x) id(%x)\n",
		    __func__, hh->prim, hh->id);
	switch (hh->prim) {
	case PH_DATA_IND:
		mISDN_FsmEvent(&mgr->deact, EV_UI, NULL);
		ret = ph_data_ind(mgr, skb);
		break;
	case PH_DATA_CNF:
		do_ack(mgr, hh->id);
		ret = 0;
		break;
	case PH_ACTIVATE_IND:
		test_and_set_bit(MGR_PH_ACTIVE, &mgr->options);
		mISDN_FsmEvent(&mgr->deact, EV_ACTIVATE_IND, NULL);
		do_send(mgr);
		ret = 0;
		break;
	case PH_DEACTIVATE_IND:
		test_and_clear_bit(MGR_PH_ACTIVE, &mgr->options);
		mISDN_FsmEvent(&mgr->deact, EV_DEACTIVATE_IND, NULL);
		ret = 0;
		break;
	case DL_UNITDATA_REQ:
		return dl_unit_data(mgr, skb);
	}
	if (!ret)
		dev_kfree_skb(skb);
	return ret;
}

static int
free_teimanager(struct manager *mgr)
{
	struct layer2	*l2, *nl2;

	test_and_clear_bit(OPTION_L1_HOLD, &mgr->options);
	if (test_bit(MGR_OPT_NETWORK, &mgr->options)) {
		/* not locked lock is taken in release tei */
		mgr->up = NULL;
		if (test_bit(OPTION_L2_CLEANUP, &mgr->options)) {
			list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
				put_tei_msg(mgr, ID_REMOVE, 0, l2->tei);
				mutex_lock(&mgr->ch.st->lmutex);
				list_del(&l2->ch.list);
				mutex_unlock(&mgr->ch.st->lmutex);
				l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
			}
			test_and_clear_bit(MGR_OPT_NETWORK, &mgr->options);
		} else {
			list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
				l2->up = NULL;
			}
		}
	}
	if (test_bit(MGR_OPT_USER, &mgr->options)) {
		if (list_empty(&mgr->layer2))
			test_and_clear_bit(MGR_OPT_USER, &mgr->options);
	}
	mgr->ch.st->dev->D.ctrl(&mgr->ch.st->dev->D, CLOSE_CHANNEL, NULL);
	return 0;
}

static int
ctrl_teimanager(struct manager *mgr, void *arg)
{
	/* currently we only have one option */
	int	*val = (int *)arg;
	int	ret = 0;

	switch (val[0]) {
	case IMCLEAR_L2:
		if (val[1])
			test_and_set_bit(OPTION_L2_CLEANUP, &mgr->options);
		else
			test_and_clear_bit(OPTION_L2_CLEANUP, &mgr->options);
		break;
	case IMHOLD_L1:
		if (val[1])
			test_and_set_bit(OPTION_L1_HOLD, &mgr->options);
		else
			test_and_clear_bit(OPTION_L1_HOLD, &mgr->options);
		break;
	default:
		ret = -EINVAL;
	}
	return ret;
}

/* This function does create a L2 for fixed TEI in NT Mode */
static int
check_data(struct manager *mgr, struct sk_buff *skb)
{
	struct mISDNhead	*hh =  mISDN_HEAD_P(skb);
	int			ret, tei, sapi;
	struct layer2		*l2;

	if (*debug & DEBUG_L2_CTRL)
		printk(KERN_DEBUG "%s: prim(%x) id(%x)\n",
		    __func__, hh->prim, hh->id);
	if (test_bit(MGR_OPT_USER, &mgr->options))
		return -ENOTCONN;
	if (hh->prim != PH_DATA_IND)
		return -ENOTCONN;
	if (skb->len != 3)
		return -ENOTCONN;
	if (skb->data[0] & 3) /* EA0 and CR must be  0 */
		return -EINVAL;
	sapi = skb->data[0] >> 2;
	if (!(skb->data[1] & 1)) /* invalid EA1 */
		return -EINVAL;
	tei = skb->data[1] >> 1;
	if (tei > 63) /* not a fixed tei */
		return -ENOTCONN;
	if ((skb->data[2] & ~0x10) != SABME)
		return -ENOTCONN;
	/* We got a SABME for a fixed TEI */
	l2 = create_new_tei(mgr, tei, sapi);
	if (!l2)
		return -ENOMEM;
	ret = l2->ch.send(&l2->ch, skb);
	return ret;
}

void
delete_teimanager(struct mISDNchannel *ch)
{
	struct manager	*mgr;
	struct layer2	*l2, *nl2;

	mgr = container_of(ch, struct manager, ch);
	/* not locked lock is taken in release tei */
	list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) {
		mutex_lock(&mgr->ch.st->lmutex);
		list_del(&l2->ch.list);
		mutex_unlock(&mgr->ch.st->lmutex);
		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
	}
	list_del(&mgr->ch.list);
	list_del(&mgr->bcast.list);
	skb_queue_purge(&mgr->sendq);
	kfree(mgr);
}

static int
mgr_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
{
	struct manager	*mgr;
	int		ret = -EINVAL;

	mgr = container_of(ch, struct manager, ch);
	if (*debug & DEBUG_L2_CTRL)
		printk(KERN_DEBUG "%s(%x, %p)\n", __func__, cmd, arg);
	switch (cmd) {
	case OPEN_CHANNEL:
		ret = create_teimgr(mgr, arg);
		break;
	case CLOSE_CHANNEL:
		ret = free_teimanager(mgr);
		break;
	case CONTROL_CHANNEL:
		ret = ctrl_teimanager(mgr, arg);
		break;
	case CHECK_DATA:
		ret = check_data(mgr, arg);
		break;
	}
	return ret;
}

static int
mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb)
{
	struct manager		*mgr = container_of(ch, struct manager, bcast);
	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
	struct sk_buff		*cskb = NULL;
	struct layer2		*l2;
	u_long			flags;
	int			ret;

	read_lock_irqsave(&mgr->lock, flags);
	list_for_each_entry(l2, &mgr->layer2, list) {
		if ((hh->id & MISDN_ID_SAPI_MASK) ==
		    (l2->ch.addr & MISDN_ID_SAPI_MASK)) {
			if (list_is_last(&l2->list, &mgr->layer2)) {
				cskb = skb;
				skb = NULL;
			} else {
				if (!cskb)
					cskb = skb_copy(skb, GFP_KERNEL);
			}
			if (cskb) {
				ret = l2->ch.send(&l2->ch, cskb);
				if (ret) {
					if (*debug & DEBUG_SEND_ERR)
						printk(KERN_DEBUG
						    "%s ch%d prim(%x) addr(%x)"
						    " err %d\n",
						    __func__, l2->ch.nr,
						    hh->prim, l2->ch.addr, ret);
				} else
					cskb = NULL;
			} else {
				printk(KERN_WARNING "%s ch%d addr %x no mem\n",
				    __func__, ch->nr, ch->addr);
				goto out;
			}
		}
	}
out:
	read_unlock_irqrestore(&mgr->lock, flags);
	if (cskb)
		dev_kfree_skb(cskb);
	if (skb)
		dev_kfree_skb(skb);
	return 0;
}

static int
mgr_bcast_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
{

	return -EINVAL;
}

int
create_teimanager(struct mISDNdevice *dev)
{
	struct manager *mgr;

	mgr = kzalloc(sizeof(struct manager), GFP_KERNEL);
	if (!mgr)
		return -ENOMEM;
	INIT_LIST_HEAD(&mgr->layer2);
	rwlock_init(&mgr->lock);
	skb_queue_head_init(&mgr->sendq);
	mgr->nextid = 1;
	mgr->lastid = MISDN_ID_NONE;
	mgr->ch.send = mgr_send;
	mgr->ch.ctrl = mgr_ctrl;
	mgr->ch.st = dev->D.st;
	set_channel_address(&mgr->ch, TEI_SAPI, GROUP_TEI);
	add_layer2(&mgr->ch, dev->D.st);
	mgr->bcast.send = mgr_bcast;
	mgr->bcast.ctrl = mgr_bcast_ctrl;
	mgr->bcast.st = dev->D.st;
	set_channel_address(&mgr->bcast, 0, GROUP_TEI);
	add_layer2(&mgr->bcast, dev->D.st);
	mgr->deact.debug = *debug & DEBUG_MANAGER;
	mgr->deact.userdata = mgr;
	mgr->deact.printdebug = da_debug;
	mgr->deact.fsm = &deactfsm;
	mgr->deact.state = ST_L1_DEACT;
	mISDN_FsmInitTimer(&mgr->deact, &mgr->datimer);
	dev->teimgr = &mgr->ch;
	return 0;
}

int TEIInit(u_int *deb)
{
	debug = deb;
	teifsmu.state_count = TEI_STATE_COUNT;
	teifsmu.event_count = TEI_EVENT_COUNT;
	teifsmu.strEvent = strTeiEvent;
	teifsmu.strState = strTeiState;
	mISDN_FsmNew(&teifsmu, TeiFnListUser, ARRAY_SIZE(TeiFnListUser));
	teifsmn.state_count = TEI_STATE_COUNT;
	teifsmn.event_count = TEI_EVENT_COUNT;
	teifsmn.strEvent = strTeiEvent;
	teifsmn.strState = strTeiState;
	mISDN_FsmNew(&teifsmn, TeiFnListNet, ARRAY_SIZE(TeiFnListNet));
	deactfsm.state_count =  DEACT_STATE_COUNT;
	deactfsm.event_count = DEACT_EVENT_COUNT;
	deactfsm.strEvent = strDeactEvent;
	deactfsm.strState = strDeactState;
	mISDN_FsmNew(&deactfsm, DeactFnList, ARRAY_SIZE(DeactFnList));
	return 0;
}

void TEIFree(void)
{
	mISDN_FsmFree(&teifsmu);
	mISDN_FsmFree(&teifsmn);
	mISDN_FsmFree(&deactfsm);
}