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
9#include <linux/slab.h>
10#include <linux/mISDNif.h>
11#include <linux/kthread.h>
12#include <linux/sched.h>
13#include <linux/sched/cputime.h>
14#include <linux/signal.h>
15
16#include "core.h"
17
18static u_int	*debug;
19
20static inline void
21_queue_message(struct mISDNstack *st, struct sk_buff *skb)
22{
23	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
24
25	if (*debug & DEBUG_QUEUE_FUNC)
26		printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
27		       __func__, hh->prim, hh->id, skb);
28	skb_queue_tail(&st->msgq, skb);
29	if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) {
30		test_and_set_bit(mISDN_STACK_WORK, &st->status);
31		wake_up_interruptible(&st->workq);
32	}
33}
34
35static int
36mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
37{
38	_queue_message(ch->st, skb);
39	return 0;
40}
41
42static struct mISDNchannel *
43get_channel4id(struct mISDNstack *st, u_int id)
44{
45	struct mISDNchannel	*ch;
46
47	mutex_lock(&st->lmutex);
48	list_for_each_entry(ch, &st->layer2, list) {
49		if (id == ch->nr)
50			goto unlock;
51	}
52	ch = NULL;
53unlock:
54	mutex_unlock(&st->lmutex);
55	return ch;
56}
57
58static void
59send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
60{
61	struct sock		*sk;
62	struct sk_buff		*cskb = NULL;
63
64	read_lock(&sl->lock);
65	sk_for_each(sk, &sl->head) {
66		if (sk->sk_state != MISDN_BOUND)
67			continue;
68		if (!cskb)
69			cskb = skb_copy(skb, GFP_ATOMIC);
70		if (!cskb) {
71			printk(KERN_WARNING "%s no skb\n", __func__);
72			break;
73		}
74		if (!sock_queue_rcv_skb(sk, cskb))
75			cskb = NULL;
76	}
77	read_unlock(&sl->lock);
78	dev_kfree_skb(cskb);
79}
80
81static void
82send_layer2(struct mISDNstack *st, struct sk_buff *skb)
83{
84	struct sk_buff		*cskb;
85	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
86	struct mISDNchannel	*ch;
87	int			ret;
88
89	if (!st)
90		return;
91	mutex_lock(&st->lmutex);
92	if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) { /* L2 for all */
93		list_for_each_entry(ch, &st->layer2, list) {
94			if (list_is_last(&ch->list, &st->layer2)) {
95				cskb = skb;
96				skb = NULL;
97			} else {
98				cskb = skb_copy(skb, GFP_KERNEL);
99			}
100			if (cskb) {
101				ret = ch->send(ch, cskb);
102				if (ret) {
103					if (*debug & DEBUG_SEND_ERR)
104						printk(KERN_DEBUG
105						       "%s ch%d prim(%x) addr(%x)"
106						       " err %d\n",
107						       __func__, ch->nr,
108						       hh->prim, ch->addr, ret);
109					dev_kfree_skb(cskb);
110				}
111			} else {
112				printk(KERN_WARNING "%s ch%d addr %x no mem\n",
113				       __func__, ch->nr, ch->addr);
114				goto out;
115			}
116		}
117	} else {
118		list_for_each_entry(ch, &st->layer2, list) {
119			if ((hh->id & MISDN_ID_ADDR_MASK) == ch->addr) {
120				ret = ch->send(ch, skb);
121				if (!ret)
122					skb = NULL;
123				goto out;
124			}
125		}
126		ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
127		if (!ret)
128			skb = NULL;
129		else if (*debug & DEBUG_SEND_ERR)
130			printk(KERN_DEBUG
131			       "%s mgr prim(%x) err %d\n",
132			       __func__, hh->prim, ret);
133	}
134out:
135	mutex_unlock(&st->lmutex);
136	dev_kfree_skb(skb);
137}
138
139static inline int
140send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
141{
142	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
143	struct mISDNchannel	*ch;
144	int	lm;
145
146	lm = hh->prim & MISDN_LAYERMASK;
147	if (*debug & DEBUG_QUEUE_FUNC)
148		printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
149		       __func__, hh->prim, hh->id, skb);
150	if (lm == 0x1) {
151		if (!hlist_empty(&st->l1sock.head)) {
152			__net_timestamp(skb);
153			send_socklist(&st->l1sock, skb);
154		}
155		return st->layer1->send(st->layer1, skb);
156	} else if (lm == 0x2) {
157		if (!hlist_empty(&st->l1sock.head))
158			send_socklist(&st->l1sock, skb);
159		send_layer2(st, skb);
160		return 0;
161	} else if (lm == 0x4) {
162		ch = get_channel4id(st, hh->id);
163		if (ch)
164			return ch->send(ch, skb);
165		else
166			printk(KERN_WARNING
167			       "%s: dev(%s) prim(%x) id(%x) no channel\n",
168			       __func__, dev_name(&st->dev->dev), hh->prim,
169			       hh->id);
170	} else if (lm == 0x8) {
171		WARN_ON(lm == 0x8);
172		ch = get_channel4id(st, hh->id);
173		if (ch)
174			return ch->send(ch, skb);
175		else
176			printk(KERN_WARNING
177			       "%s: dev(%s) prim(%x) id(%x) no channel\n",
178			       __func__, dev_name(&st->dev->dev), hh->prim,
179			       hh->id);
180	} else {
181		/* broadcast not handled yet */
182		printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
183		       __func__, dev_name(&st->dev->dev), hh->prim);
184	}
185	return -ESRCH;
186}
187
188static void
189do_clear_stack(struct mISDNstack *st)
190{
191}
192
193static int
194mISDNStackd(void *data)
195{
196	struct mISDNstack *st = data;
197#ifdef MISDN_MSG_STATS
198	u64 utime, stime;
199#endif
200	int err = 0;
201
202	sigfillset(&current->blocked);
203	if (*debug & DEBUG_MSG_THREAD)
204		printk(KERN_DEBUG "mISDNStackd %s started\n",
205		       dev_name(&st->dev->dev));
206
207	if (st->notify != NULL) {
208		complete(st->notify);
209		st->notify = NULL;
210	}
211
212	for (;;) {
213		struct sk_buff	*skb;
214
215		if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
216			test_and_clear_bit(mISDN_STACK_WORK, &st->status);
217			test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
218		} else
219			test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
220		while (test_bit(mISDN_STACK_WORK, &st->status)) {
221			skb = skb_dequeue(&st->msgq);
222			if (!skb) {
223				test_and_clear_bit(mISDN_STACK_WORK,
224						   &st->status);
225				/* test if a race happens */
226				skb = skb_dequeue(&st->msgq);
227				if (!skb)
228					continue;
229				test_and_set_bit(mISDN_STACK_WORK,
230						 &st->status);
231			}
232#ifdef MISDN_MSG_STATS
233			st->msg_cnt++;
234#endif
235			err = send_msg_to_layer(st, skb);
236			if (unlikely(err)) {
237				if (*debug & DEBUG_SEND_ERR)
238					printk(KERN_DEBUG
239					       "%s: %s prim(%x) id(%x) "
240					       "send call(%d)\n",
241					       __func__, dev_name(&st->dev->dev),
242					       mISDN_HEAD_PRIM(skb),
243					       mISDN_HEAD_ID(skb), err);
244				dev_kfree_skb(skb);
245				continue;
246			}
247			if (unlikely(test_bit(mISDN_STACK_STOPPED,
248					      &st->status))) {
249				test_and_clear_bit(mISDN_STACK_WORK,
250						   &st->status);
251				test_and_clear_bit(mISDN_STACK_RUNNING,
252						   &st->status);
253				break;
254			}
255		}
256		if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
257			test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
258			test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
259			do_clear_stack(st);
260			test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
261			test_and_set_bit(mISDN_STACK_RESTART, &st->status);
262		}
263		if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
264			test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
265			test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
266			if (!skb_queue_empty(&st->msgq))
267				test_and_set_bit(mISDN_STACK_WORK,
268						 &st->status);
269		}
270		if (test_bit(mISDN_STACK_ABORT, &st->status))
271			break;
272		if (st->notify != NULL) {
273			complete(st->notify);
274			st->notify = NULL;
275		}
276#ifdef MISDN_MSG_STATS
277		st->sleep_cnt++;
278#endif
279		test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
280		wait_event_interruptible(st->workq, (st->status &
281						     mISDN_STACK_ACTION_MASK));
282		if (*debug & DEBUG_MSG_THREAD)
283			printk(KERN_DEBUG "%s: %s wake status %08lx\n",
284			       __func__, dev_name(&st->dev->dev), st->status);
285		test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
286
287		test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
288
289		if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
290			test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
291#ifdef MISDN_MSG_STATS
292			st->stopped_cnt++;
293#endif
294		}
295	}
296#ifdef MISDN_MSG_STATS
297	printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
298	       "msg %d sleep %d stopped\n",
299	       dev_name(&st->dev->dev), st->msg_cnt, st->sleep_cnt,
300	       st->stopped_cnt);
301	task_cputime(st->thread, &utime, &stime);
302	printk(KERN_DEBUG
303	       "mISDNStackd daemon for %s utime(%llu) stime(%llu)\n",
304	       dev_name(&st->dev->dev), utime, stime);
305	printk(KERN_DEBUG
306	       "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
307	       dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
308	printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
309	       dev_name(&st->dev->dev));
310#endif
311	test_and_set_bit(mISDN_STACK_KILLED, &st->status);
312	test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
313	test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
314	test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
315	skb_queue_purge(&st->msgq);
316	st->thread = NULL;
317	if (st->notify != NULL) {
318		complete(st->notify);
319		st->notify = NULL;
320	}
321	return 0;
322}
323
324static int
325l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
326{
327	if (!ch->st)
328		return -ENODEV;
329	__net_timestamp(skb);
330	_queue_message(ch->st, skb);
331	return 0;
332}
333
334void
335set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
336{
337	ch->addr = sapi | (tei << 8);
338}
339
340void
341__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
342{
343	list_add_tail(&ch->list, &st->layer2);
344}
345
346void
347add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
348{
349	mutex_lock(&st->lmutex);
350	__add_layer2(ch, st);
351	mutex_unlock(&st->lmutex);
352}
353
354static int
355st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
356{
357	if (!ch->st || !ch->st->layer1)
358		return -EINVAL;
359	return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
360}
361
362int
363create_stack(struct mISDNdevice *dev)
364{
365	struct mISDNstack	*newst;
366	int			err;
367	DECLARE_COMPLETION_ONSTACK(done);
368
369	newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
370	if (!newst) {
371		printk(KERN_ERR "kmalloc mISDN_stack failed\n");
372		return -ENOMEM;
373	}
374	newst->dev = dev;
375	INIT_LIST_HEAD(&newst->layer2);
376	INIT_HLIST_HEAD(&newst->l1sock.head);
377	rwlock_init(&newst->l1sock.lock);
378	init_waitqueue_head(&newst->workq);
379	skb_queue_head_init(&newst->msgq);
380	mutex_init(&newst->lmutex);
381	dev->D.st = newst;
382	err = create_teimanager(dev);
383	if (err) {
384		printk(KERN_ERR "kmalloc teimanager failed\n");
385		kfree(newst);
386		return err;
387	}
388	dev->teimgr->peer = &newst->own;
389	dev->teimgr->recv = mISDN_queue_message;
390	dev->teimgr->st = newst;
391	newst->layer1 = &dev->D;
392	dev->D.recv = l1_receive;
393	dev->D.peer = &newst->own;
394	newst->own.st = newst;
395	newst->own.ctrl = st_own_ctrl;
396	newst->own.send = mISDN_queue_message;
397	newst->own.recv = mISDN_queue_message;
398	if (*debug & DEBUG_CORE_FUNC)
399		printk(KERN_DEBUG "%s: st(%s)\n", __func__,
400		       dev_name(&newst->dev->dev));
401	newst->notify = &done;
402	newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
403				    dev_name(&newst->dev->dev));
404	if (IS_ERR(newst->thread)) {
405		err = PTR_ERR(newst->thread);
406		printk(KERN_ERR
407		       "mISDN:cannot create kernel thread for %s (%d)\n",
408		       dev_name(&newst->dev->dev), err);
409		delete_teimanager(dev->teimgr);
410		kfree(newst);
411	} else
412		wait_for_completion(&done);
413	return err;
414}
415
416int
417connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
418	       u_int protocol, struct sockaddr_mISDN *adr)
419{
420	struct mISDN_sock	*msk = container_of(ch, struct mISDN_sock, ch);
421	struct channel_req	rq;
422	int			err;
423
424
425	if (*debug &  DEBUG_CORE_FUNC)
426		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
427		       __func__, dev_name(&dev->dev), protocol, adr->dev,
428		       adr->channel, adr->sapi, adr->tei);
429	switch (protocol) {
430	case ISDN_P_NT_S0:
431	case ISDN_P_NT_E1:
432	case ISDN_P_TE_S0:
433	case ISDN_P_TE_E1:
434		ch->recv = mISDN_queue_message;
435		ch->peer = &dev->D.st->own;
436		ch->st = dev->D.st;
437		rq.protocol = protocol;
438		rq.adr.channel = adr->channel;
439		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
440		printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
441		       dev->id);
442		if (err)
443			return err;
444		write_lock_bh(&dev->D.st->l1sock.lock);
445		sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
446		write_unlock_bh(&dev->D.st->l1sock.lock);
447		break;
448	default:
449		return -ENOPROTOOPT;
450	}
451	return 0;
452}
453
454int
455connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
456	       u_int protocol, struct sockaddr_mISDN *adr)
457{
458	struct channel_req	rq, rq2;
459	int			pmask, err;
460	struct Bprotocol	*bp;
461
462	if (*debug &  DEBUG_CORE_FUNC)
463		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
464		       __func__, dev_name(&dev->dev), protocol,
465		       adr->dev, adr->channel, adr->sapi,
466		       adr->tei);
467	ch->st = dev->D.st;
468	pmask = 1 << (protocol & ISDN_P_B_MASK);
469	if (pmask & dev->Bprotocols) {
470		rq.protocol = protocol;
471		rq.adr = *adr;
472		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
473		if (err)
474			return err;
475		ch->recv = rq.ch->send;
476		ch->peer = rq.ch;
477		rq.ch->recv = ch->send;
478		rq.ch->peer = ch;
479		rq.ch->st = dev->D.st;
480	} else {
481		bp = get_Bprotocol4mask(pmask);
482		if (!bp)
483			return -ENOPROTOOPT;
484		rq2.protocol = protocol;
485		rq2.adr = *adr;
486		rq2.ch = ch;
487		err = bp->create(&rq2);
488		if (err)
489			return err;
490		ch->recv = rq2.ch->send;
491		ch->peer = rq2.ch;
492		rq2.ch->st = dev->D.st;
493		rq.protocol = rq2.protocol;
494		rq.adr = *adr;
495		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
496		if (err) {
497			rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
498			return err;
499		}
500		rq2.ch->recv = rq.ch->send;
501		rq2.ch->peer = rq.ch;
502		rq.ch->recv = rq2.ch->send;
503		rq.ch->peer = rq2.ch;
504		rq.ch->st = dev->D.st;
505	}
506	ch->protocol = protocol;
507	ch->nr = rq.ch->nr;
508	return 0;
509}
510
511int
512create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
513		u_int protocol, struct sockaddr_mISDN *adr)
514{
515	struct channel_req	rq;
516	int			err;
517
518	if (*debug &  DEBUG_CORE_FUNC)
519		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
520		       __func__, dev_name(&dev->dev), protocol,
521		       adr->dev, adr->channel, adr->sapi,
522		       adr->tei);
523	rq.protocol = ISDN_P_TE_S0;
524	if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
525		rq.protocol = ISDN_P_TE_E1;
526	switch (protocol) {
527	case ISDN_P_LAPD_NT:
528		rq.protocol = ISDN_P_NT_S0;
529		if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
530			rq.protocol = ISDN_P_NT_E1;
531		fallthrough;
532	case ISDN_P_LAPD_TE:
533		ch->recv = mISDN_queue_message;
534		ch->peer = &dev->D.st->own;
535		ch->st = dev->D.st;
536		rq.adr.channel = 0;
537		err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
538		printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
539		if (err)
540			break;
541		rq.protocol = protocol;
542		rq.adr = *adr;
543		rq.ch = ch;
544		err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
545		printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
546		if (!err) {
547			if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
548				break;
549			add_layer2(rq.ch, dev->D.st);
550			rq.ch->recv = mISDN_queue_message;
551			rq.ch->peer = &dev->D.st->own;
552			rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
553		}
554		break;
555	default:
556		err = -EPROTONOSUPPORT;
557	}
558	return err;
559}
560
561void
562delete_channel(struct mISDNchannel *ch)
563{
564	struct mISDN_sock	*msk = container_of(ch, struct mISDN_sock, ch);
565	struct mISDNchannel	*pch;
566
567	if (!ch->st) {
568		printk(KERN_WARNING "%s: no stack\n", __func__);
569		return;
570	}
571	if (*debug & DEBUG_CORE_FUNC)
572		printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
573		       dev_name(&ch->st->dev->dev), ch->protocol);
574	if (ch->protocol >= ISDN_P_B_START) {
575		if (ch->peer) {
576			ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
577			ch->peer = NULL;
578		}
579		return;
580	}
581	switch (ch->protocol) {
582	case ISDN_P_NT_S0:
583	case ISDN_P_TE_S0:
584	case ISDN_P_NT_E1:
585	case ISDN_P_TE_E1:
586		write_lock_bh(&ch->st->l1sock.lock);
587		sk_del_node_init(&msk->sk);
588		write_unlock_bh(&ch->st->l1sock.lock);
589		ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
590		break;
591	case ISDN_P_LAPD_TE:
592		pch = get_channel4id(ch->st, ch->nr);
593		if (pch) {
594			mutex_lock(&ch->st->lmutex);
595			list_del(&pch->list);
596			mutex_unlock(&ch->st->lmutex);
597			pch->ctrl(pch, CLOSE_CHANNEL, NULL);
598			pch = ch->st->dev->teimgr;
599			pch->ctrl(pch, CLOSE_CHANNEL, NULL);
600		} else
601			printk(KERN_WARNING "%s: no l2 channel\n",
602			       __func__);
603		break;
604	case ISDN_P_LAPD_NT:
605		pch = ch->st->dev->teimgr;
606		if (pch) {
607			pch->ctrl(pch, CLOSE_CHANNEL, NULL);
608		} else
609			printk(KERN_WARNING "%s: no l2 channel\n",
610			       __func__);
611		break;
612	default:
613		break;
614	}
615	return;
616}
617
618void
619delete_stack(struct mISDNdevice *dev)
620{
621	struct mISDNstack	*st = dev->D.st;
622	DECLARE_COMPLETION_ONSTACK(done);
623
624	if (*debug & DEBUG_CORE_FUNC)
625		printk(KERN_DEBUG "%s: st(%s)\n", __func__,
626		       dev_name(&st->dev->dev));
627	if (dev->teimgr)
628		delete_teimanager(dev->teimgr);
629	if (st->thread) {
630		if (st->notify) {
631			printk(KERN_WARNING "%s: notifier in use\n",
632			       __func__);
633			complete(st->notify);
634		}
635		st->notify = &done;
636		test_and_set_bit(mISDN_STACK_ABORT, &st->status);
637		test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
638		wake_up_interruptible(&st->workq);
639		wait_for_completion(&done);
640	}
641	if (!list_empty(&st->layer2))
642		printk(KERN_WARNING "%s: layer2 list not empty\n",
643		       __func__);
644	if (!hlist_empty(&st->l1sock.head))
645		printk(KERN_WARNING "%s: layer1 list not empty\n",
646		       __func__);
647	kfree(st);
648}
649
650void
651mISDN_initstack(u_int *dp)
652{
653	debug = dp;
654}
655