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
)