1 // SPDX-License-Identifier: GPL-2.0-only
4 * Author Karsten Keil <kkeil@novell.com>
6 * Copyright 2008 by Karsten Keil <kkeil@novell.com>
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>
21 _queue_message(struct mISDNstack *st, struct sk_buff *skb)
23 struct mISDNhead *hh = mISDN_HEAD_P(skb);
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);
36 mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
38 _queue_message(ch->st, skb);
42 static struct mISDNchannel *
43 get_channel4id(struct mISDNstack *st, u_int id)
45 struct mISDNchannel *ch;
47 mutex_lock(&st->lmutex);
48 list_for_each_entry(ch, &st->layer2, list) {
54 mutex_unlock(&st->lmutex);
59 send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
62 struct sk_buff *cskb = NULL;
65 sk_for_each(sk, &sl->head) {
66 if (sk->sk_state != MISDN_BOUND)
69 cskb = skb_copy(skb, GFP_ATOMIC);
71 printk(KERN_WARNING "%s no skb\n", __func__);
74 if (!sock_queue_rcv_skb(sk, cskb))
77 read_unlock(&sl->lock);
82 send_layer2(struct mISDNstack *st, struct sk_buff *skb)
85 struct mISDNhead *hh = mISDN_HEAD_P(skb);
86 struct mISDNchannel *ch;
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)) {
98 cskb = skb_copy(skb, GFP_KERNEL);
101 ret = ch->send(ch, cskb);
103 if (*debug & DEBUG_SEND_ERR)
105 "%s ch%d prim(%x) addr(%x)"
108 hh->prim, ch->addr, ret);
112 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
113 __func__, ch->nr, ch->addr);
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);
126 ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
129 else if (*debug & DEBUG_SEND_ERR)
131 "%s mgr prim(%x) err %d\n",
132 __func__, hh->prim, ret);
135 mutex_unlock(&st->lmutex);
140 send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
142 struct mISDNhead *hh = mISDN_HEAD_P(skb);
143 struct mISDNchannel *ch;
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);
151 if (!hlist_empty(&st->l1sock.head)) {
152 __net_timestamp(skb);
153 send_socklist(&st->l1sock, skb);
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);
161 } else if (lm == 0x4) {
162 ch = get_channel4id(st, hh->id);
164 return ch->send(ch, skb);
167 "%s: dev(%s) prim(%x) id(%x) no channel\n",
168 __func__, dev_name(&st->dev->dev), hh->prim,
170 } else if (lm == 0x8) {
172 ch = get_channel4id(st, hh->id);
174 return ch->send(ch, skb);
177 "%s: dev(%s) prim(%x) id(%x) no channel\n",
178 __func__, dev_name(&st->dev->dev), hh->prim,
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);
189 do_clear_stack(struct mISDNstack *st)
194 mISDNStackd(void *data)
196 struct mISDNstack *st = data;
197 #ifdef MISDN_MSG_STATS
202 sigfillset(¤t->blocked);
203 if (*debug & DEBUG_MSG_THREAD)
204 printk(KERN_DEBUG "mISDNStackd %s started\n",
205 dev_name(&st->dev->dev));
207 if (st->notify != NULL) {
208 complete(st->notify);
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);
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);
223 test_and_clear_bit(mISDN_STACK_WORK,
225 /* test if a race happens */
226 skb = skb_dequeue(&st->msgq);
229 test_and_set_bit(mISDN_STACK_WORK,
232 #ifdef MISDN_MSG_STATS
235 err = send_msg_to_layer(st, skb);
237 if (*debug & DEBUG_SEND_ERR)
239 "%s: %s prim(%x) id(%x) "
241 __func__, dev_name(&st->dev->dev),
242 mISDN_HEAD_PRIM(skb),
243 mISDN_HEAD_ID(skb), err);
247 if (unlikely(test_bit(mISDN_STACK_STOPPED,
249 test_and_clear_bit(mISDN_STACK_WORK,
251 test_and_clear_bit(mISDN_STACK_RUNNING,
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);
260 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
261 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
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,
270 if (test_bit(mISDN_STACK_ABORT, &st->status))
272 if (st->notify != NULL) {
273 complete(st->notify);
276 #ifdef MISDN_MSG_STATS
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);
287 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
289 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
290 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
291 #ifdef MISDN_MSG_STATS
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,
301 task_cputime(st->thread, &utime, &stime);
303 "mISDNStackd daemon for %s utime(%llu) stime(%llu)\n",
304 dev_name(&st->dev->dev), utime, stime);
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));
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);
317 if (st->notify != NULL) {
318 complete(st->notify);
325 l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
329 __net_timestamp(skb);
330 _queue_message(ch->st, skb);
335 set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
337 ch->addr = sapi | (tei << 8);
341 __add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
343 list_add_tail(&ch->list, &st->layer2);
347 add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
349 mutex_lock(&st->lmutex);
350 __add_layer2(ch, st);
351 mutex_unlock(&st->lmutex);
355 st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
357 if (!ch->st || !ch->st->layer1)
359 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
363 create_stack(struct mISDNdevice *dev)
365 struct mISDNstack *newst;
367 DECLARE_COMPLETION_ONSTACK(done);
369 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
371 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
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);
382 err = create_teimanager(dev);
384 printk(KERN_ERR "kmalloc teimanager failed\n");
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);
407 "mISDN:cannot create kernel thread for %s (%d)\n",
408 dev_name(&newst->dev->dev), err);
409 delete_teimanager(dev->teimgr);
412 wait_for_completion(&done);
417 connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
418 u_int protocol, struct sockaddr_mISDN *adr)
420 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
421 struct channel_req rq;
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);
434 ch->recv = mISDN_queue_message;
435 ch->peer = &dev->D.st->own;
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,
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);
455 connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
456 u_int protocol, struct sockaddr_mISDN *adr)
458 struct channel_req rq, rq2;
460 struct Bprotocol *bp;
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,
468 pmask = 1 << (protocol & ISDN_P_B_MASK);
469 if (pmask & dev->Bprotocols) {
470 rq.protocol = protocol;
472 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
475 ch->recv = rq.ch->send;
477 rq.ch->recv = ch->send;
479 rq.ch->st = dev->D.st;
481 bp = get_Bprotocol4mask(pmask);
484 rq2.protocol = protocol;
487 err = bp->create(&rq2);
490 ch->recv = rq2.ch->send;
492 rq2.ch->st = dev->D.st;
493 rq.protocol = rq2.protocol;
495 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
497 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
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;
506 ch->protocol = protocol;
512 create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
513 u_int protocol, struct sockaddr_mISDN *adr)
515 struct channel_req rq;
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,
523 rq.protocol = ISDN_P_TE_S0;
524 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
525 rq.protocol = ISDN_P_TE_E1;
528 rq.protocol = ISDN_P_NT_S0;
529 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
530 rq.protocol = ISDN_P_NT_E1;
533 ch->recv = mISDN_queue_message;
534 ch->peer = &dev->D.st->own;
537 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
538 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
541 rq.protocol = protocol;
544 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
545 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
547 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
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 */
556 err = -EPROTONOSUPPORT;
562 delete_channel(struct mISDNchannel *ch)
564 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
565 struct mISDNchannel *pch;
568 printk(KERN_WARNING "%s: no stack\n", __func__);
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) {
576 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
581 switch (ch->protocol) {
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);
592 pch = get_channel4id(ch->st, ch->nr);
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);
601 printk(KERN_WARNING "%s: no l2 channel\n",
605 pch = ch->st->dev->teimgr;
607 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
609 printk(KERN_WARNING "%s: no l2 channel\n",
619 delete_stack(struct mISDNdevice *dev)
621 struct mISDNstack *st = dev->D.st;
622 DECLARE_COMPLETION_ONSTACK(done);
624 if (*debug & DEBUG_CORE_FUNC)
625 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
626 dev_name(&st->dev->dev));
628 delete_teimanager(dev->teimgr);
631 printk(KERN_WARNING "%s: notifier in use\n",
633 complete(st->notify);
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);
641 if (!list_empty(&st->layer2))
642 printk(KERN_WARNING "%s: layer2 list not empty\n",
644 if (!hlist_empty(&st->l1sock.head))
645 printk(KERN_WARNING "%s: layer1 list not empty\n",
651 mISDN_initstack(u_int *dp)