3 * Author Karsten Keil <kkeil@novell.com>
5 * Copyright 2008 by Karsten Keil <kkeil@novell.com>
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.
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.
18 #include <linux/slab.h>
19 #include <linux/mISDNif.h>
20 #include <linux/kthread.h>
21 #include <linux/sched.h>
27 _queue_message(struct mISDNstack
*st
, struct sk_buff
*skb
)
29 struct mISDNhead
*hh
= mISDN_HEAD_P(skb
);
31 if (*debug
& DEBUG_QUEUE_FUNC
)
32 printk(KERN_DEBUG
"%s prim(%x) id(%x) %p\n",
33 __func__
, hh
->prim
, hh
->id
, skb
);
34 skb_queue_tail(&st
->msgq
, skb
);
35 if (likely(!test_bit(mISDN_STACK_STOPPED
, &st
->status
))) {
36 test_and_set_bit(mISDN_STACK_WORK
, &st
->status
);
37 wake_up_interruptible(&st
->workq
);
42 mISDN_queue_message(struct mISDNchannel
*ch
, struct sk_buff
*skb
)
44 _queue_message(ch
->st
, skb
);
48 static struct mISDNchannel
*
49 get_channel4id(struct mISDNstack
*st
, u_int id
)
51 struct mISDNchannel
*ch
;
53 mutex_lock(&st
->lmutex
);
54 list_for_each_entry(ch
, &st
->layer2
, list
) {
60 mutex_unlock(&st
->lmutex
);
65 send_socklist(struct mISDN_sock_list
*sl
, struct sk_buff
*skb
)
68 struct sk_buff
*cskb
= NULL
;
71 sk_for_each(sk
, &sl
->head
) {
72 if (sk
->sk_state
!= MISDN_BOUND
)
75 cskb
= skb_copy(skb
, GFP_KERNEL
);
77 printk(KERN_WARNING
"%s no skb\n", __func__
);
80 if (!sock_queue_rcv_skb(sk
, cskb
))
83 read_unlock(&sl
->lock
);
89 send_layer2(struct mISDNstack
*st
, struct sk_buff
*skb
)
92 struct mISDNhead
*hh
= mISDN_HEAD_P(skb
);
93 struct mISDNchannel
*ch
;
98 mutex_lock(&st
->lmutex
);
99 if ((hh
->id
& MISDN_ID_ADDR_MASK
) == MISDN_ID_ANY
) { /* L2 for all */
100 list_for_each_entry(ch
, &st
->layer2
, list
) {
101 if (list_is_last(&ch
->list
, &st
->layer2
)) {
105 cskb
= skb_copy(skb
, GFP_KERNEL
);
108 ret
= ch
->send(ch
, cskb
);
110 if (*debug
& DEBUG_SEND_ERR
)
112 "%s ch%d prim(%x) addr(%x)"
115 hh
->prim
, ch
->addr
, ret
);
119 printk(KERN_WARNING
"%s ch%d addr %x no mem\n",
120 __func__
, ch
->nr
, ch
->addr
);
125 list_for_each_entry(ch
, &st
->layer2
, list
) {
126 if ((hh
->id
& MISDN_ID_ADDR_MASK
) == ch
->addr
) {
127 ret
= ch
->send(ch
, skb
);
133 ret
= st
->dev
->teimgr
->ctrl(st
->dev
->teimgr
, CHECK_DATA
, skb
);
136 else if (*debug
& DEBUG_SEND_ERR
)
138 "%s mgr prim(%x) err %d\n",
139 __func__
, hh
->prim
, ret
);
142 mutex_unlock(&st
->lmutex
);
148 send_msg_to_layer(struct mISDNstack
*st
, struct sk_buff
*skb
)
150 struct mISDNhead
*hh
= mISDN_HEAD_P(skb
);
151 struct mISDNchannel
*ch
;
154 lm
= hh
->prim
& MISDN_LAYERMASK
;
155 if (*debug
& DEBUG_QUEUE_FUNC
)
156 printk(KERN_DEBUG
"%s prim(%x) id(%x) %p\n",
157 __func__
, hh
->prim
, hh
->id
, skb
);
159 if (!hlist_empty(&st
->l1sock
.head
)) {
160 __net_timestamp(skb
);
161 send_socklist(&st
->l1sock
, skb
);
163 return st
->layer1
->send(st
->layer1
, skb
);
164 } else if (lm
== 0x2) {
165 if (!hlist_empty(&st
->l1sock
.head
))
166 send_socklist(&st
->l1sock
, skb
);
167 send_layer2(st
, skb
);
169 } else if (lm
== 0x4) {
170 ch
= get_channel4id(st
, hh
->id
);
172 return ch
->send(ch
, skb
);
175 "%s: dev(%s) prim(%x) id(%x) no channel\n",
176 __func__
, dev_name(&st
->dev
->dev
), hh
->prim
,
178 } else if (lm
== 0x8) {
180 ch
= get_channel4id(st
, hh
->id
);
182 return ch
->send(ch
, skb
);
185 "%s: dev(%s) prim(%x) id(%x) no channel\n",
186 __func__
, dev_name(&st
->dev
->dev
), hh
->prim
,
189 /* broadcast not handled yet */
190 printk(KERN_WARNING
"%s: dev(%s) prim %x not delivered\n",
191 __func__
, dev_name(&st
->dev
->dev
), hh
->prim
);
197 do_clear_stack(struct mISDNstack
*st
)
202 mISDNStackd(void *data
)
204 struct mISDNstack
*st
= data
;
205 #ifdef MISDN_MSG_STATS
206 cputime_t utime
, stime
;
210 sigfillset(¤t
->blocked
);
211 if (*debug
& DEBUG_MSG_THREAD
)
212 printk(KERN_DEBUG
"mISDNStackd %s started\n",
213 dev_name(&st
->dev
->dev
));
215 if (st
->notify
!= NULL
) {
216 complete(st
->notify
);
223 if (unlikely(test_bit(mISDN_STACK_STOPPED
, &st
->status
))) {
224 test_and_clear_bit(mISDN_STACK_WORK
, &st
->status
);
225 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
227 test_and_set_bit(mISDN_STACK_RUNNING
, &st
->status
);
228 while (test_bit(mISDN_STACK_WORK
, &st
->status
)) {
229 skb
= skb_dequeue(&st
->msgq
);
231 test_and_clear_bit(mISDN_STACK_WORK
,
233 /* test if a race happens */
234 skb
= skb_dequeue(&st
->msgq
);
237 test_and_set_bit(mISDN_STACK_WORK
,
240 #ifdef MISDN_MSG_STATS
243 err
= send_msg_to_layer(st
, skb
);
245 if (*debug
& DEBUG_SEND_ERR
)
247 "%s: %s prim(%x) id(%x) "
249 __func__
, dev_name(&st
->dev
->dev
),
250 mISDN_HEAD_PRIM(skb
),
251 mISDN_HEAD_ID(skb
), err
);
255 if (unlikely(test_bit(mISDN_STACK_STOPPED
,
257 test_and_clear_bit(mISDN_STACK_WORK
,
259 test_and_clear_bit(mISDN_STACK_RUNNING
,
264 if (test_bit(mISDN_STACK_CLEARING
, &st
->status
)) {
265 test_and_set_bit(mISDN_STACK_STOPPED
, &st
->status
);
266 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
268 test_and_clear_bit(mISDN_STACK_CLEARING
, &st
->status
);
269 test_and_set_bit(mISDN_STACK_RESTART
, &st
->status
);
271 if (test_and_clear_bit(mISDN_STACK_RESTART
, &st
->status
)) {
272 test_and_clear_bit(mISDN_STACK_STOPPED
, &st
->status
);
273 test_and_set_bit(mISDN_STACK_RUNNING
, &st
->status
);
274 if (!skb_queue_empty(&st
->msgq
))
275 test_and_set_bit(mISDN_STACK_WORK
,
278 if (test_bit(mISDN_STACK_ABORT
, &st
->status
))
280 if (st
->notify
!= NULL
) {
281 complete(st
->notify
);
284 #ifdef MISDN_MSG_STATS
287 test_and_clear_bit(mISDN_STACK_ACTIVE
, &st
->status
);
288 wait_event_interruptible(st
->workq
, (st
->status
&
289 mISDN_STACK_ACTION_MASK
));
290 if (*debug
& DEBUG_MSG_THREAD
)
291 printk(KERN_DEBUG
"%s: %s wake status %08lx\n",
292 __func__
, dev_name(&st
->dev
->dev
), st
->status
);
293 test_and_set_bit(mISDN_STACK_ACTIVE
, &st
->status
);
295 test_and_clear_bit(mISDN_STACK_WAKEUP
, &st
->status
);
297 if (test_bit(mISDN_STACK_STOPPED
, &st
->status
)) {
298 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
299 #ifdef MISDN_MSG_STATS
304 #ifdef MISDN_MSG_STATS
305 printk(KERN_DEBUG
"mISDNStackd daemon for %s proceed %d "
306 "msg %d sleep %d stopped\n",
307 dev_name(&st
->dev
->dev
), st
->msg_cnt
, st
->sleep_cnt
,
309 task_cputime(st
->thread
, &utime
, &stime
);
311 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
312 dev_name(&st
->dev
->dev
), utime
, stime
);
314 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
315 dev_name(&st
->dev
->dev
), st
->thread
->nvcsw
, st
->thread
->nivcsw
);
316 printk(KERN_DEBUG
"mISDNStackd daemon for %s killed now\n",
317 dev_name(&st
->dev
->dev
));
319 test_and_set_bit(mISDN_STACK_KILLED
, &st
->status
);
320 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
321 test_and_clear_bit(mISDN_STACK_ACTIVE
, &st
->status
);
322 test_and_clear_bit(mISDN_STACK_ABORT
, &st
->status
);
323 skb_queue_purge(&st
->msgq
);
325 if (st
->notify
!= NULL
) {
326 complete(st
->notify
);
333 l1_receive(struct mISDNchannel
*ch
, struct sk_buff
*skb
)
337 __net_timestamp(skb
);
338 _queue_message(ch
->st
, skb
);
343 set_channel_address(struct mISDNchannel
*ch
, u_int sapi
, u_int tei
)
345 ch
->addr
= sapi
| (tei
<< 8);
349 __add_layer2(struct mISDNchannel
*ch
, struct mISDNstack
*st
)
351 list_add_tail(&ch
->list
, &st
->layer2
);
355 add_layer2(struct mISDNchannel
*ch
, struct mISDNstack
*st
)
357 mutex_lock(&st
->lmutex
);
358 __add_layer2(ch
, st
);
359 mutex_unlock(&st
->lmutex
);
363 st_own_ctrl(struct mISDNchannel
*ch
, u_int cmd
, void *arg
)
365 if (!ch
->st
|| !ch
->st
->layer1
)
367 return ch
->st
->layer1
->ctrl(ch
->st
->layer1
, cmd
, arg
);
371 create_stack(struct mISDNdevice
*dev
)
373 struct mISDNstack
*newst
;
375 DECLARE_COMPLETION_ONSTACK(done
);
377 newst
= kzalloc(sizeof(struct mISDNstack
), GFP_KERNEL
);
379 printk(KERN_ERR
"kmalloc mISDN_stack failed\n");
383 INIT_LIST_HEAD(&newst
->layer2
);
384 INIT_HLIST_HEAD(&newst
->l1sock
.head
);
385 rwlock_init(&newst
->l1sock
.lock
);
386 init_waitqueue_head(&newst
->workq
);
387 skb_queue_head_init(&newst
->msgq
);
388 mutex_init(&newst
->lmutex
);
390 err
= create_teimanager(dev
);
392 printk(KERN_ERR
"kmalloc teimanager failed\n");
396 dev
->teimgr
->peer
= &newst
->own
;
397 dev
->teimgr
->recv
= mISDN_queue_message
;
398 dev
->teimgr
->st
= newst
;
399 newst
->layer1
= &dev
->D
;
400 dev
->D
.recv
= l1_receive
;
401 dev
->D
.peer
= &newst
->own
;
402 newst
->own
.st
= newst
;
403 newst
->own
.ctrl
= st_own_ctrl
;
404 newst
->own
.send
= mISDN_queue_message
;
405 newst
->own
.recv
= mISDN_queue_message
;
406 if (*debug
& DEBUG_CORE_FUNC
)
407 printk(KERN_DEBUG
"%s: st(%s)\n", __func__
,
408 dev_name(&newst
->dev
->dev
));
409 newst
->notify
= &done
;
410 newst
->thread
= kthread_run(mISDNStackd
, (void *)newst
, "mISDN_%s",
411 dev_name(&newst
->dev
->dev
));
412 if (IS_ERR(newst
->thread
)) {
413 err
= PTR_ERR(newst
->thread
);
415 "mISDN:cannot create kernel thread for %s (%d)\n",
416 dev_name(&newst
->dev
->dev
), err
);
417 delete_teimanager(dev
->teimgr
);
420 wait_for_completion(&done
);
425 connect_layer1(struct mISDNdevice
*dev
, struct mISDNchannel
*ch
,
426 u_int protocol
, struct sockaddr_mISDN
*adr
)
428 struct mISDN_sock
*msk
= container_of(ch
, struct mISDN_sock
, ch
);
429 struct channel_req rq
;
433 if (*debug
& DEBUG_CORE_FUNC
)
434 printk(KERN_DEBUG
"%s: %s proto(%x) adr(%d %d %d %d)\n",
435 __func__
, dev_name(&dev
->dev
), protocol
, adr
->dev
,
436 adr
->channel
, adr
->sapi
, adr
->tei
);
442 ch
->recv
= mISDN_queue_message
;
443 ch
->peer
= &dev
->D
.st
->own
;
445 rq
.protocol
= protocol
;
446 rq
.adr
.channel
= adr
->channel
;
447 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
448 printk(KERN_DEBUG
"%s: ret %d (dev %d)\n", __func__
, err
,
452 write_lock_bh(&dev
->D
.st
->l1sock
.lock
);
453 sk_add_node(&msk
->sk
, &dev
->D
.st
->l1sock
.head
);
454 write_unlock_bh(&dev
->D
.st
->l1sock
.lock
);
463 connect_Bstack(struct mISDNdevice
*dev
, struct mISDNchannel
*ch
,
464 u_int protocol
, struct sockaddr_mISDN
*adr
)
466 struct channel_req rq
, rq2
;
468 struct Bprotocol
*bp
;
470 if (*debug
& DEBUG_CORE_FUNC
)
471 printk(KERN_DEBUG
"%s: %s proto(%x) adr(%d %d %d %d)\n",
472 __func__
, dev_name(&dev
->dev
), protocol
,
473 adr
->dev
, adr
->channel
, adr
->sapi
,
476 pmask
= 1 << (protocol
& ISDN_P_B_MASK
);
477 if (pmask
& dev
->Bprotocols
) {
478 rq
.protocol
= protocol
;
480 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
483 ch
->recv
= rq
.ch
->send
;
485 rq
.ch
->recv
= ch
->send
;
487 rq
.ch
->st
= dev
->D
.st
;
489 bp
= get_Bprotocol4mask(pmask
);
492 rq2
.protocol
= protocol
;
495 err
= bp
->create(&rq2
);
498 ch
->recv
= rq2
.ch
->send
;
500 rq2
.ch
->st
= dev
->D
.st
;
501 rq
.protocol
= rq2
.protocol
;
503 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
505 rq2
.ch
->ctrl(rq2
.ch
, CLOSE_CHANNEL
, NULL
);
508 rq2
.ch
->recv
= rq
.ch
->send
;
509 rq2
.ch
->peer
= rq
.ch
;
510 rq
.ch
->recv
= rq2
.ch
->send
;
511 rq
.ch
->peer
= rq2
.ch
;
512 rq
.ch
->st
= dev
->D
.st
;
514 ch
->protocol
= protocol
;
520 create_l2entity(struct mISDNdevice
*dev
, struct mISDNchannel
*ch
,
521 u_int protocol
, struct sockaddr_mISDN
*adr
)
523 struct channel_req rq
;
526 if (*debug
& DEBUG_CORE_FUNC
)
527 printk(KERN_DEBUG
"%s: %s proto(%x) adr(%d %d %d %d)\n",
528 __func__
, dev_name(&dev
->dev
), protocol
,
529 adr
->dev
, adr
->channel
, adr
->sapi
,
531 rq
.protocol
= ISDN_P_TE_S0
;
532 if (dev
->Dprotocols
& (1 << ISDN_P_TE_E1
))
533 rq
.protocol
= ISDN_P_TE_E1
;
536 rq
.protocol
= ISDN_P_NT_S0
;
537 if (dev
->Dprotocols
& (1 << ISDN_P_NT_E1
))
538 rq
.protocol
= ISDN_P_NT_E1
;
540 ch
->recv
= mISDN_queue_message
;
541 ch
->peer
= &dev
->D
.st
->own
;
544 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
545 printk(KERN_DEBUG
"%s: ret 1 %d\n", __func__
, err
);
548 rq
.protocol
= protocol
;
551 err
= dev
->teimgr
->ctrl(dev
->teimgr
, OPEN_CHANNEL
, &rq
);
552 printk(KERN_DEBUG
"%s: ret 2 %d\n", __func__
, err
);
554 if ((protocol
== ISDN_P_LAPD_NT
) && !rq
.ch
)
556 add_layer2(rq
.ch
, dev
->D
.st
);
557 rq
.ch
->recv
= mISDN_queue_message
;
558 rq
.ch
->peer
= &dev
->D
.st
->own
;
559 rq
.ch
->ctrl(rq
.ch
, OPEN_CHANNEL
, NULL
); /* can't fail */
563 err
= -EPROTONOSUPPORT
;
569 delete_channel(struct mISDNchannel
*ch
)
571 struct mISDN_sock
*msk
= container_of(ch
, struct mISDN_sock
, ch
);
572 struct mISDNchannel
*pch
;
575 printk(KERN_WARNING
"%s: no stack\n", __func__
);
578 if (*debug
& DEBUG_CORE_FUNC
)
579 printk(KERN_DEBUG
"%s: st(%s) protocol(%x)\n", __func__
,
580 dev_name(&ch
->st
->dev
->dev
), ch
->protocol
);
581 if (ch
->protocol
>= ISDN_P_B_START
) {
583 ch
->peer
->ctrl(ch
->peer
, CLOSE_CHANNEL
, NULL
);
588 switch (ch
->protocol
) {
593 write_lock_bh(&ch
->st
->l1sock
.lock
);
594 sk_del_node_init(&msk
->sk
);
595 write_unlock_bh(&ch
->st
->l1sock
.lock
);
596 ch
->st
->dev
->D
.ctrl(&ch
->st
->dev
->D
, CLOSE_CHANNEL
, NULL
);
599 pch
= get_channel4id(ch
->st
, ch
->nr
);
601 mutex_lock(&ch
->st
->lmutex
);
602 list_del(&pch
->list
);
603 mutex_unlock(&ch
->st
->lmutex
);
604 pch
->ctrl(pch
, CLOSE_CHANNEL
, NULL
);
605 pch
= ch
->st
->dev
->teimgr
;
606 pch
->ctrl(pch
, CLOSE_CHANNEL
, NULL
);
608 printk(KERN_WARNING
"%s: no l2 channel\n",
612 pch
= ch
->st
->dev
->teimgr
;
614 pch
->ctrl(pch
, CLOSE_CHANNEL
, NULL
);
616 printk(KERN_WARNING
"%s: no l2 channel\n",
626 delete_stack(struct mISDNdevice
*dev
)
628 struct mISDNstack
*st
= dev
->D
.st
;
629 DECLARE_COMPLETION_ONSTACK(done
);
631 if (*debug
& DEBUG_CORE_FUNC
)
632 printk(KERN_DEBUG
"%s: st(%s)\n", __func__
,
633 dev_name(&st
->dev
->dev
));
635 delete_teimanager(dev
->teimgr
);
638 printk(KERN_WARNING
"%s: notifier in use\n",
640 complete(st
->notify
);
643 test_and_set_bit(mISDN_STACK_ABORT
, &st
->status
);
644 test_and_set_bit(mISDN_STACK_WAKEUP
, &st
->status
);
645 wake_up_interruptible(&st
->workq
);
646 wait_for_completion(&done
);
648 if (!list_empty(&st
->layer2
))
649 printk(KERN_WARNING
"%s: layer2 list not empty\n",
651 if (!hlist_empty(&st
->l1sock
.head
))
652 printk(KERN_WARNING
"%s: layer1 list not empty\n",
658 mISDN_initstack(u_int
*dp
)