xref: /linux/drivers/isdn/mISDN/tei.c (revision d39d0ed196aa1685bb24771e92f78633c66ac9cb)
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 already 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 }
1366