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 
28 static	u_int	*debug;
29 
30 static struct Fsm deactfsm = {NULL, 0, 0, NULL, NULL};
31 static struct Fsm teifsmu = {NULL, 0, 0, NULL, NULL};
32 static struct Fsm teifsmn = {NULL, 0, 0, NULL, NULL};
33 
34 enum {
35 	ST_L1_DEACT,
36 	ST_L1_DEACT_PENDING,
37 	ST_L1_ACTIV,
38 };
39 #define DEACT_STATE_COUNT (ST_L1_ACTIV + 1)
40 
41 static char *strDeactState[] =
42 {
43 	"ST_L1_DEACT",
44 	"ST_L1_DEACT_PENDING",
45 	"ST_L1_ACTIV",
46 };
47 
48 enum {
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 
59 static 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 
69 static void
da_debug(struct FsmInst * fi,char * fmt,...)70 da_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 
89 static void
da_activate(struct FsmInst * fi,int event,void * arg)90 da_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 
99 static void
da_deactivate_ind(struct FsmInst * fi,int event,void * arg)100 da_deactivate_ind(struct FsmInst *fi, int event, void *arg)
101 {
102 	mISDN_FsmChangeState(fi, ST_L1_DEACT);
103 }
104 
105 static void
da_deactivate(struct FsmInst * fi,int event,void * arg)106 da_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 
129 static void
da_ui(struct FsmInst * fi,int event,void * arg)130 da_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 
142 static void
da_timer(struct FsmInst * fi,int event,void * arg)143 da_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 
166 static 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 
176 enum {
177 	ST_TEI_NOP,
178 	ST_TEI_IDREQ,
179 	ST_TEI_IDVERIFY,
180 };
181 
182 #define TEI_STATE_COUNT (ST_TEI_IDVERIFY + 1)
183 
184 static char *strTeiState[] =
185 {
186 	"ST_TEI_NOP",
187 	"ST_TEI_IDREQ",
188 	"ST_TEI_IDVERIFY",
189 };
190 
191 enum {
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 
205 static 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 
218 static void
tei_debug(struct FsmInst * fi,char * fmt,...)219 tei_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 
241 static int
get_free_id(struct manager * mgr)242 get_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 
265 static int
get_free_tei(struct manager * mgr)266 get_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 
292 static void
teiup_create(struct manager * mgr,u_int prim,int len,void * arg)293 teiup_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 
314 static u_int
new_id(struct manager * mgr)315 new_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 
328 static void
do_send(struct manager * mgr)329 do_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 
351 static void
do_ack(struct manager * mgr,u_int id)352 do_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 
373 static void
mgr_send_down(struct manager * mgr,struct sk_buff * skb)374 mgr_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 
385 static int
dl_unit_data(struct manager * mgr,struct sk_buff * skb)386 dl_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 
404 static unsigned int
random_ri(void)405 random_ri(void)
406 {
407 	u16 x;
408 
409 	get_random_bytes(&x, sizeof(x));
410 	return x;
411 }
412 
413 static struct layer2 *
findtei(struct manager * mgr,int tei)414 findtei(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;
426 done:
427 	read_unlock_irqrestore(&mgr->lock, flags);
428 	return l2;
429 }
430 
431 static void
put_tei_msg(struct manager * mgr,u_char m_id,unsigned int ri,int tei)432 put_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 
455 static void
tei_id_request(struct FsmInst * fi,int event,void * arg)456 tei_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 
476 static void
tei_id_assign(struct FsmInst * fi,int event,void * arg)477 tei_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 
505 static void
tei_id_test_dup(struct FsmInst * fi,int event,void * arg)506 tei_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 
530 static void
tei_id_denied(struct FsmInst * fi,int event,void * arg)531 tei_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 
546 static void
tei_id_chk_req(struct FsmInst * fi,int event,void * arg)547 tei_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 
564 static void
tei_id_remove(struct FsmInst * fi,int event,void * arg)565 tei_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 
582 static void
tei_id_verify(struct FsmInst * fi,int event,void * arg)583 tei_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 
596 static void
tei_id_req_tout(struct FsmInst * fi,int event,void * arg)597 tei_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 
615 static void
tei_id_ver_tout(struct FsmInst * fi,int event,void * arg)616 tei_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 
635 static 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 
650 static void
tei_l2remove(struct layer2 * l2)651 tei_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 
659 static void
tei_assign_req(struct FsmInst * fi,int event,void * arg)660 tei_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 
679 static void
tei_id_chk_req_net(struct FsmInst * fi,int event,void * arg)680 tei_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 
694 static void
tei_id_chk_resp(struct FsmInst * fi,int event,void * arg)695 tei_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 
708 static void
tei_id_verify_net(struct FsmInst * fi,int event,void * arg)709 tei_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 
723 static void
tei_id_ver_tout_net(struct FsmInst * fi,int event,void * arg)724 tei_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 
751 static 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 
760 static void
tei_ph_data_ind(struct teimgr * tm,u_int mt,u_char * dp,int len)761 tei_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 
781 static struct layer2 *
create_new_tei(struct manager * mgr,int tei,int sapi)782 create_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 
847 static void
new_tei_req(struct manager * mgr,u_char * dp)848 new_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;
873 denied:
874 	put_tei_msg(mgr, ID_DENIED, ri, GROUP_TEI);
875 }
876 
877 static int
ph_data_ind(struct manager * mgr,struct sk_buff * skb)878 ph_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 	}
929 done:
930 	return ret;
931 }
932 
933 int
l2_tei(struct layer2 * l2,u_int cmd,u_long arg)934 l2_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 
968 void
TEIrelease(struct layer2 * l2)969 TEIrelease(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 
982 static int
create_teimgr(struct manager * mgr,struct channel_req * crq)983 create_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 
1095 static int
mgr_send(struct mISDNchannel * ch,struct sk_buff * skb)1096 mgr_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 
1138 static int
free_teimanager(struct manager * mgr)1139 free_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 
1170 static int
ctrl_teimanager(struct manager * mgr,void * arg)1171 ctrl_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 */
1196 static int
check_data(struct manager * mgr,struct sk_buff * skb)1197 check_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 
1237 void
delete_teimanager(struct mISDNchannel * ch)1238 delete_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 
1257 static int
mgr_ctrl(struct mISDNchannel * ch,u_int cmd,void * arg)1258 mgr_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 
1283 static int
mgr_bcast(struct mISDNchannel * ch,struct sk_buff * skb)1284 mgr_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 	}
1329 out:
1330 	read_unlock_irqrestore(&mgr->lock, flags);
1331 	dev_kfree_skb(cskb);
1332 	dev_kfree_skb(skb);
1333 	return 0;
1334 }
1335 
1336 static int
mgr_bcast_ctrl(struct mISDNchannel * ch,u_int cmd,void * arg)1337 mgr_bcast_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
1338 {
1339 
1340 	return -EINVAL;
1341 }
1342 
1343 int
create_teimanager(struct mISDNdevice * dev)1344 create_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 
TEIInit(u_int * deb)1376 int 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 
1403 error_deact:
1404 	mISDN_FsmFree(&teifsmn);
1405 error_smn:
1406 	mISDN_FsmFree(&teifsmu);
1407 error:
1408 	return res;
1409 }
1410 
TEIFree(void)1411 void TEIFree(void)
1412 {
1413 	mISDN_FsmFree(&teifsmu);
1414 	mISDN_FsmFree(&teifsmn);
1415 	mISDN_FsmFree(&deactfsm);
1416 }
1417