isdnl2.c

arm平台上的uclinux系统全部源代码

	struct PStack *st = fi->userdata;

	if (test_bit(FLG_LAPD, &st->l2.flag) &&
		test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
		FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 9);
		return;
	}
	test_and_clear_bit(FLG_T200_RUN, &st->l2.flag);
	if (fi->state == ST_L2_7) {
		st->l2.rc = 0;
		FsmChangeState(fi, ST_L2_8);
	}
	if (st->l2.rc == st->l2.N200) {
		establishlink(fi);
	} else {
		transmit_enquiry(st);
		st->l2.rc++;
	}
}

static void
l2_st7_tout_203(struct FsmInst *fi, int event, void *arg)
{
	struct PStack *st = fi->userdata;

	if (test_bit(FLG_LAPD, &st->l2.flag) &&
		test_bit(FLG_DCHAN_BUSY, &st->l2.flag)) {
		FsmAddTimer(&st->l2.t203, st->l2.T203, EV_L2_T203, NULL, 9);
		return;
	}
	FsmChangeState(fi, ST_L2_8);
	transmit_enquiry(st);
	st->l2.rc = 0;
}

static void
l2_pull_iqueue(struct FsmInst *fi, int event, void *arg)
{
	struct PStack *st = fi->userdata;
	struct sk_buff *skb, *oskb;
	struct Layer2 *l2 = &st->l2;
	u_char header[MAX_HEADER_LEN];
	int p1, i;

	if (!cansend(st))
		return;

	skb = skb_dequeue(&l2->i_queue);
	if (!skb)
		return;

	p1 = l2->vs - l2->va;
	if (p1 < 0)
		p1 += test_bit(FLG_MOD128, &l2->flag) ? 128 : 8;
	p1 = (p1 + l2->sow) % l2->window;
	if (l2->windowar[p1]) {
		printk(KERN_WARNING "isdnl2 try overwrite ack queue entry %d\n",
		       p1);
		dev_kfree_skb(l2->windowar[p1], FREE_WRITE);
	}
	l2->windowar[p1] = skb_clone(skb, GFP_ATOMIC);

	i = sethdraddr(&st->l2, header, CMD);

	if (test_bit(FLG_MOD128, &l2->flag)) {
		header[i++] = l2->vs << 1;
		header[i++] = l2->vr << 1;
		l2->vs = (l2->vs + 1) % 128;
	} else {
		header[i++] = (l2->vr << 5) | (l2->vs << 1);
		l2->vs = (l2->vs + 1) % 8;
	}
	p1 = skb->data - skb->head;
	if (p1 >= i)
		memcpy(skb_push(skb, i), header, i);
	else {
		printk(KERN_WARNING
		"isdl2 pull_iqueue skb header(%d/%d) too short\n", i, p1);
		oskb = skb;
		skb = alloc_skb(oskb->len + i, GFP_ATOMIC);
		SET_SKB_FREE(skb);
		memcpy(skb_put(skb, i), header, i);
		memcpy(skb_put(skb, oskb->len), oskb->data, oskb->len);
		FreeSkb(oskb);
	}
	st->l2.l2l1(st, PH_PULL | INDICATION, skb);
	test_and_clear_bit(FLG_ACK_PEND, &st->l2.flag);
	if (!test_and_set_bit(FLG_T200_RUN, &st->l2.flag)) {
		FsmDelTimer(&st->l2.t203, 13);
		FsmAddTimer(&st->l2.t200, st->l2.T200, EV_L2_T200, NULL, 11);
	}
	if (skb_queue_len(&l2->i_queue) && cansend(st))
		st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
}

static void
l2_got_st8_super(struct FsmInst *fi, int event, void *arg)
{
	struct PStack *st = fi->userdata;
	struct sk_buff *skb = arg;
	int PollFlag, nr, rsp, rnr = 0;
	struct Layer2 *l2 = &st->l2;

	rsp = *skb->data & 0x2;
	if (test_bit(FLG_ORIG, &l2->flag))
		rsp = !rsp;
	skb_pull(skb, l2addrsize(l2));

	if (IsRNR(skb->data, test_bit(FLG_MOD128, &l2->flag))) {
		test_and_set_bit(FLG_PEER_BUSY, &l2->flag);
		rnr = 1;
	} else
		test_and_clear_bit(FLG_PEER_BUSY, &l2->flag);
	if (test_bit(FLG_MOD128, &l2->flag)) {
		if (skb->len == 2) {
			PollFlag = (skb->data[1] & 0x1) == 0x1;
			nr = skb->data[1] >> 1;
		} else {
			st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'N');
			FreeSkb(skb);
			establishlink(fi);
			return;
		}
	} else {
		if (skb->len == 1) {
			PollFlag = (skb->data[0] & 0x10);
			nr = (skb->data[0] >> 5) & 0x7;
		} else {
			st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'N');
			FreeSkb(skb);
			establishlink(fi);
			return;
		}
	}
	FreeSkb(skb);

	if (rsp && PollFlag) {
		if (legalnr(st, nr)) {
			setva(st, nr);
			if (test_and_clear_bit(FLG_T200_RUN, &st->l2.flag))
				FsmDelTimer(&st->l2.t200, 7);
			FsmDelTimer(&l2->t203, 8);
			if (rnr) {
				FsmRestartTimer(&l2->t200, l2->T200,
						EV_L2_T200, NULL, 14);
				test_and_set_bit(FLG_T200_RUN, &st->l2.flag);
			} else
				FsmAddTimer(&l2->t203, l2->T203,
					    EV_L2_T203, NULL, 5);
			invoke_retransmission(st, nr);
			FsmChangeState(fi, ST_L2_7);
			if (skb_queue_len(&l2->i_queue) && cansend(st))
				st->l2.l2l1(st, PH_PULL | REQUEST, NULL);
			else if (fi->userint & LC_FLUSH_WAIT) {
				fi->userint &= ~LC_FLUSH_WAIT;
				st->l2.l2l3(st, DL_FLUSH | INDICATION, NULL);
			}
		}
	} else {
		if (!rsp && PollFlag)
			enquiry_response(st);
		if (legalnr(st, nr)) {
			setva(st, nr);
		}
	}
}

static void
l2_got_FRMR(struct FsmInst *fi, int event, void *arg)
{
	struct PStack *st = fi->userdata;
	struct sk_buff *skb = arg;

	skb_pull(skb, l2addrsize(&st->l2) + 1);
	if (test_bit(FLG_MOD128, &st->l2.flag)) {
		if (skb->len < 5)
			st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'N');
		else
			l2m_debug(&st->l2.l2m, "FRMR information %2x %2x %2x %2x %2x",
				skb->data[0], skb->data[1], skb->data[2],
				skb->data[3], skb->data[4]);
	} else {
		if (skb->len < 3)
			st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'N');
		else
			l2m_debug(&st->l2.l2m, "FRMR information %2x %2x %2x",
				skb->data[0], skb->data[1], skb->data[2]);
	}
	if (!(skb->data[0] & 1) || ((skb->data[0] & 3) == 1) ||		/* I or S */
	    (IsUA(skb->data, 0) && (fi->state == ST_L2_7))) {
		st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'K');
		establishlink(fi);
		test_and_clear_bit(FLG_L3_INIT, &st->l2.flag);
	}
	FreeSkb(skb);
}

static void
l2_tei_remove(struct FsmInst *fi, int event, void *arg)
{
	struct PStack *st = fi->userdata;

	discard_queue(&st->l2.i_queue);
	discard_queue(&st->l2.ui_queue);
	st->l2.tei = -1;
	if (test_and_clear_bit(FLG_T200_RUN, &st->l2.flag))
		FsmDelTimer(&st->l2.t200, 18);
	FsmDelTimer(&st->l2.t203, 19);
	if (fi->state != ST_L2_4)
		st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
	FsmChangeState(fi, ST_L2_1);
}

static void
l2_persistant_da(struct FsmInst *fi, int event, void *arg)
{
	struct PStack *st = fi->userdata;
	int rel = DL_RELEASE | INDICATION;
	
	
	discard_queue(&st->l2.i_queue);
	discard_queue(&st->l2.ui_queue);
	if (test_and_clear_bit(FLG_T200_RUN, &st->l2.flag))
		FsmDelTimer(&st->l2.t200, 18);
	FsmDelTimer(&st->l2.t203, 19);
	clear_exception(&st->l2);
	switch (fi->state) {
		case ST_L2_1:
			if (!test_and_clear_bit(FLG_ESTAB_PEND, &st->l2.flag))
				break;
		case ST_L2_3:
			st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
		case ST_L2_2:
			FsmChangeState(fi, ST_L2_1);
			break;
		case ST_L2_6:
			rel = DL_RELEASE | CONFIRM;
		case ST_L2_5:
			if (test_and_clear_bit(FLG_PEND_REL, &st->l2.flag))
				rel = DL_RELEASE | CONFIRM;
		case ST_L2_7:
		case ST_L2_8:
			st->l2.l2l3(st, rel, NULL);
			FsmChangeState(fi, ST_L2_4);
			break;
		case ST_L2_4:
			if (test_and_clear_bit(FLG_ESTAB_PEND, &st->l2.flag))
				st->l2.l2l3(st, DL_RELEASE | INDICATION, NULL);
			break;
	}
	test_and_clear_bit(FLG_PEND_REL, &st->l2.flag);
	test_and_clear_bit(FLG_L1_ACTIV, &st->l2.flag);
}

static struct FsmNode L2FnList[] HISAX_INITDATA =
{
	{ST_L2_1, EV_L2_DL_ESTABLISH, l2_dl_establish},
	{ST_L2_2, EV_L2_DL_ESTABLISH, l2_dl_establish},
	{ST_L2_4, EV_L2_DL_ESTABLISH, l2_establish},
	{ST_L2_5, EV_L2_DL_ESTABLISH, l2_establish},
	{ST_L2_7, EV_L2_DL_ESTABLISH, l2_establish},
	{ST_L2_8, EV_L2_DL_ESTABLISH, l2_establish},
	{ST_L2_4, EV_L2_DL_RELEASE, l2_dl_release},
	{ST_L2_5, EV_L2_DL_RELEASE, l2_dl_release},
	{ST_L2_7, EV_L2_DL_RELEASE, l2_dl_release},
	{ST_L2_8, EV_L2_DL_RELEASE, l2_dl_release},
	{ST_L2_5, EV_L2_DL_DATA, l2_feed_iqueue},
	{ST_L2_7, EV_L2_DL_DATA, l2_feed_iqueue},
	{ST_L2_8, EV_L2_DL_DATA, l2_feed_iqueue},
	{ST_L2_1, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_2, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_3, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_4, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_5, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_6, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_7, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_8, EV_L2_DL_UNIT_DATA, l2_put_ui},
	{ST_L2_1, EV_L2_MDL_ASSIGN, l2_got_tei},
	{ST_L2_2, EV_L2_MDL_ASSIGN, l2_got_tei},
	{ST_L2_3, EV_L2_MDL_ASSIGN, l2_got_tei},
	{ST_L2_2, EV_L2_MDL_ERROR, l2_tei_remove},
	{ST_L2_3, EV_L2_MDL_ERROR, l2_tei_remove},
	{ST_L2_4, EV_L2_MDL_REMOVE, l2_tei_remove},
	{ST_L2_5, EV_L2_MDL_REMOVE, l2_tei_remove},
	{ST_L2_6, EV_L2_MDL_REMOVE, l2_tei_remove},
	{ST_L2_7, EV_L2_MDL_REMOVE, l2_tei_remove},
	{ST_L2_8, EV_L2_MDL_REMOVE, l2_tei_remove},
	{ST_L2_4, EV_L2_SABMX, l2_got_SABMX},
	{ST_L2_5, EV_L2_SABMX, l2_got_SABMX},
	{ST_L2_6, EV_L2_SABMX, l2_got_SABMX},
	{ST_L2_7, EV_L2_SABMX, l2_got_SABMX},
	{ST_L2_8, EV_L2_SABMX, l2_got_SABMX},
	{ST_L2_4, EV_L2_DISC, l2_got_disconn},
	{ST_L2_5, EV_L2_DISC, l2_got_disconn},
	{ST_L2_6, EV_L2_DISC, l2_got_disconn},
	{ST_L2_7, EV_L2_DISC, l2_got_disconn},
	{ST_L2_8, EV_L2_DISC, l2_got_disconn},
	{ST_L2_4, EV_L2_UA, l2_mdl_error},
	{ST_L2_5, EV_L2_UA, l2_got_ua},
	{ST_L2_6, EV_L2_UA, l2_got_ua},
	{ST_L2_7, EV_L2_UA, l2_mdl_error},
	{ST_L2_8, EV_L2_UA, l2_mdl_error},
	{ST_L2_4, EV_L2_DM, l2_got_dm},
	{ST_L2_5, EV_L2_DM, l2_got_dm},
	{ST_L2_6, EV_L2_DM, l2_got_dm},
	{ST_L2_7, EV_L2_DM, l2_got_dm},
	{ST_L2_8, EV_L2_DM, l2_got_dm},
	{ST_L2_1, EV_L2_UI, l2_got_ui},
	{ST_L2_2, EV_L2_UI, l2_got_ui},
	{ST_L2_3, EV_L2_UI, l2_got_ui},
	{ST_L2_4, EV_L2_UI, l2_got_ui},
	{ST_L2_5, EV_L2_UI, l2_got_ui},
	{ST_L2_6, EV_L2_UI, l2_got_ui},
	{ST_L2_7, EV_L2_UI, l2_got_ui},
	{ST_L2_8, EV_L2_UI, l2_got_ui},
	{ST_L2_7, EV_L2_FRMR, l2_got_FRMR},
	{ST_L2_8, EV_L2_FRMR, l2_got_FRMR},
	{ST_L2_7, EV_L2_SUPER, l2_got_st7_super},
	{ST_L2_8, EV_L2_SUPER, l2_got_st8_super},
	{ST_L2_7, EV_L2_I, l2_got_iframe},
	{ST_L2_8, EV_L2_I, l2_got_iframe},
	{ST_L2_5, EV_L2_T200, l2_st5_tout_200},
	{ST_L2_6, EV_L2_T200, l2_st6_tout_200},
	{ST_L2_7, EV_L2_T200, l2_st78_tout_200},
	{ST_L2_8, EV_L2_T200, l2_st78_tout_200},
	{ST_L2_7, EV_L2_T203, l2_st7_tout_203},
	{ST_L2_7, EV_L2_ACK_PULL, l2_pull_iqueue},
	{ST_L2_1, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_2, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_3, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_4, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_5, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_6, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_7, EV_L1_DEACTIVATE, l2_persistant_da},
	{ST_L2_8, EV_L1_DEACTIVATE, l2_persistant_da},
};

#define L2_FN_COUNT (sizeof(L2FnList)/sizeof(struct FsmNode))

static void
isdnl2_l1l2(struct PStack *st, int pr, void *arg)
{
	struct sk_buff *skb = arg;
	u_char *datap;
	int ret = 1, len;

	switch (pr) {
		case (PH_DATA | INDICATION):
			datap = skb->data;
			len = l2addrsize(&st->l2);
			if (skb->len > len)
				datap += len;
			else {
				st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'N');
				FreeSkb(skb);
				return;
			}
			if (!(*datap & 1))	/* I-Frame */
				ret = FsmEvent(&st->l2.l2m, EV_L2_I, skb);
			else if (IsSFrame(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_SUPER, skb);
			else if (IsUI(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_UI, skb);
			else if (IsSABMX(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_SABMX, skb);
			else if (IsUA(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_UA, skb);
			else if (IsDISC(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_DISC, skb);
			else if (IsDM(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_DM, skb);
			else if (IsFRMR(datap, test_bit(FLG_MOD128, &st->l2.flag)))
				ret = FsmEvent(&st->l2.l2m, EV_L2_FRMR, skb);
			else {
				ret = 1;
				if ((st->l2.l2m.state == ST_L2_7) ||
					(st->l2.l2m.state == ST_L2_8))
					establishlink(&st->l2.l2m);
				st->ma.layer(st, MDL_ERROR | INDICATION, (void *) 'L');
			}
			if (ret) {
				FreeSkb(skb);
			}
			break;
		case (PH_PULL | CONFIRM):
			FsmEvent(&st->l2.l2m, EV_L2_ACK_PULL, arg);
			break;
		case (PH_PAUSE | INDICATION):
			test_and_set_bit(FLG_DCHAN_BUSY, &st->l2.flag);
			break;
		case (PH_PAUSE | CONFIRM):
			test_and_clear_bit(FLG_DCHAN_BUSY, &st->l2.flag);
			break;
		case (PH_ACTIVATE | CONFIRM):
		case (PH_ACTIVATE | INDICATION):
			test_and_set_bit(FLG_L1_ACTIV, &st->l2.flag);
			if (test_and_clear_bit(FLG_ESTAB_PEND, &st->l2.flag))
				FsmEvent(&st->l2.l2m, EV_L2_DL_ESTABLISH, arg);
			break;
		case (PH_DEACTIVATE | INDICATION):
		case (PH_DEACTIVATE | CONFIRM):
			test_and_clear_bit(FLG_L1_ACTIV, &st->l2.flag);
			FsmEvent(&st->l2.l2m, EV_L1_DEACTIVATE, arg);
			break;
		default:
			l2m_debug(&st->l2.l2m, "l2 unknown pr %04x", pr);
			break;
	}
}

static void
isdnl2_l3l2(struct PStack *st, int pr, void *arg)
{
	switch (pr) {
		case (DL_DATA | REQUEST):
			if (FsmEvent(&st->l2.l2m, EV_L2_DL_DATA, arg)) {
				dev_kfree_skb((struct sk_buff *) arg, FREE_READ);
			}
			break;
		case (DL_UNIT_DATA | REQUEST):
			if (FsmEvent(&st->l2.l2m, EV_L2_DL_UNIT_DATA, arg)) {
				dev_kfree_skb((struct sk_buff *) arg, FREE_READ);
			}
			break;
		case (DL_ESTABLISH | REQUEST):
			if (test_bit(FLG_L1_ACTIV, &st->l2.flag)) {
				if (test_bit(FLG_LAPD, &st->l2.flag) ||
					test_bit(FLG_ORIG, &st->l2.flag)) {
					FsmEvent(&st->l2.l2m, EV_L2_DL_ESTABLISH, arg);
				}
			} else {
				if (test_bit(FLG_LAPD, &st->l2.flag) ||
					test_bit(FLG_ORIG, &st->l2.flag)) {
					test_and_set_bit(FLG_ESTAB_PEND, &st->l2.flag);
				}
				st->l2.l2l1(st, PH_ACTIVATE, NULL);
			}
			break;
		case (DL_RELEASE | REQUEST):
			if (test_bit(FLG_LAPB, &st->l2.flag)) {
				st->l2.l2l1(st, PH_DEACTIVATE, NULL);
			}
			FsmEvent(&st->l2.l2m, EV_L2_DL_RELEASE, arg);
			break;
		case (DL_FLUSH | REQUEST):
			(&st->l2.l2m)->userint |= LC_FLUSH_WAIT;
			break;
		case (MDL_ASSIGN | REQUEST):
			FsmEvent(&st->l2.l2m, EV_L2_MDL_ASSIGN, arg);
			break;
		case (MDL_REMOVE | REQUEST):
			FsmEvent(&st->l2.l2m, EV_L2_MDL_REMOVE, arg);
			break;
		case (MDL_ERROR | RESPONSE):
			FsmEvent(&st->l2.l2m, EV_L2_MDL_ERROR, arg);
			break;
	}
}

void
releasestack_isdnl2(struct PStack *st)
{
	FsmDelTimer(&st->l2.t200, 15);
	FsmDelTimer(&st->l2.t203, 16);
	discard_queue(&st->l2.i_queue);
	discard_queue(&st->l2.ui_queue);
	ReleaseWin(&st->l2);
}

static void
l2m_debug(struct FsmInst *fi, char *fmt, ...)
{
	va_list args;
	struct PStack *st = fi->userdata;

	va_start(args, fmt);
	VHiSax_putstatus(st->l1.hardware, st->l2.debug_id, fmt, args);
	va_end(args);
}

void
setstack_isdnl2(struct PStack *st, char *debug_id)
{
	st->l1.l1l2 = isdnl2_l1l2;
	st->l3.l3l2 = isdnl2_l3l2;

	skb_queue_head_init(&st->l2.i_queue);
	skb_queue_head_init(&st->l2.ui_queue);
	InitWin(&st->l2);
	st->l2.debug = 0;

	st->l2.l2m.fsm = &l2fsm;
	if (test_bit(FLG_LAPB, &st->l2.flag))
		st->l2.l2m.state = ST_L2_4;
	else
	st->l2.l2m.state = ST_L2_1;
	st->l2.l2m.debug = 0;
	st->l2.l2m.userdata = st;
	st->l2.l2m.userint = 0;
	st->l2.l2m.printdebug = l2m_debug;
	strcpy(st->l2.debug_id, debug_id);

	FsmInitTimer(&st->l2.l2m, &st->l2.t200);
	FsmInitTimer(&st->l2.l2m, &st->l2.t203);
}

static void
transl2_l3l2(struct PStack *st, int pr, void *arg)
{
	switch (pr) {
		case (DL_DATA | REQUEST):
		case (DL_UNIT_DATA | REQUEST):
			st->l2.l2l1(st, PH_DATA | REQUEST, arg);
			break;
		case (DL_ESTABLISH | REQUEST):
			st->l2.l2l1(st, PH_ACTIVATE | REQUEST, NULL);
			break;
		case (DL_RELEASE | REQUEST):
			st->l2.l2l1(st, PH_DEACTIVATE | REQUEST, NULL);
			break;
	}
}

void
setstack_transl2(struct PStack *st)
{
	st->l3.l3l2 = transl2_l3l2;
}

void
releasestack_transl2(struct PStack *st)
{
}

HISAX_INITFUNC(void
Isdnl2New(void))
{
	l2fsm.state_count = L2_STATE_COUNT;
	l2fsm.event_count = L2_EVENT_COUNT;
	l2fsm.strEvent = strL2Event;
	l2fsm.strState = strL2State;
	FsmNew(&l2fsm, L2FnList, L2_FN_COUNT);
}

void
Isdnl2Free(void)
{
	FsmFree(&l2fsm);
}