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/mISDNif.h>
19 #include <linux/kthread.h>
25 _queue_message(struct mISDNstack
*st
, struct sk_buff
*skb
)
27 struct mISDNhead
*hh
= mISDN_HEAD_P(skb
);
29 if (*debug
& DEBUG_QUEUE_FUNC
)
30 printk(KERN_DEBUG
"%s prim(%x) id(%x) %p\n",
31 __func__
, hh
->prim
, hh
->id
, skb
);
32 skb_queue_tail(&st
->msgq
, skb
);
33 if (likely(!test_bit(mISDN_STACK_STOPPED
, &st
->status
))) {
34 test_and_set_bit(mISDN_STACK_WORK
, &st
->status
);
35 wake_up_interruptible(&st
->workq
);
40 mISDN_queue_message(struct mISDNchannel
*ch
, struct sk_buff
*skb
)
42 _queue_message(ch
->st
, skb
);
46 static struct mISDNchannel
*
47 get_channel4id(struct mISDNstack
*st
, u_int id
)
49 struct mISDNchannel
*ch
;
51 mutex_lock(&st
->lmutex
);
52 list_for_each_entry(ch
, &st
->layer2
, list
) {
58 mutex_unlock(&st
->lmutex
);
63 send_socklist(struct mISDN_sock_list
*sl
, struct sk_buff
*skb
)
65 struct hlist_node
*node
;
67 struct sk_buff
*cskb
= NULL
;
70 sk_for_each(sk
, node
, &sl
->head
) {
71 if (sk
->sk_state
!= MISDN_BOUND
)
74 cskb
= skb_copy(skb
, GFP_KERNEL
);
76 printk(KERN_WARNING
"%s no skb\n", __func__
);
79 if (!sock_queue_rcv_skb(sk
, cskb
))
82 read_unlock(&sl
->lock
);
88 send_layer2(struct mISDNstack
*st
, struct sk_buff
*skb
)
91 struct mISDNhead
*hh
= mISDN_HEAD_P(skb
);
92 struct mISDNchannel
*ch
;
97 mutex_lock(&st
->lmutex
);
98 if ((hh
->id
& MISDN_ID_ADDR_MASK
) == MISDN_ID_ANY
) { /* L2 for all */
99 list_for_each_entry(ch
, &st
->layer2
, list
) {
100 if (list_is_last(&ch
->list
, &st
->layer2
)) {
104 cskb
= skb_copy(skb
, GFP_KERNEL
);
107 ret
= ch
->send(ch
, cskb
);
109 if (*debug
& DEBUG_SEND_ERR
)
111 "%s ch%d prim(%x) addr(%x)"
114 hh
->prim
, ch
->addr
, ret
);
118 printk(KERN_WARNING
"%s ch%d addr %x no mem\n",
119 __func__
, ch
->nr
, ch
->addr
);
124 list_for_each_entry(ch
, &st
->layer2
, list
) {
125 if ((hh
->id
& MISDN_ID_ADDR_MASK
) == ch
->addr
) {
126 ret
= ch
->send(ch
, skb
);
132 ret
= st
->dev
->teimgr
->ctrl(st
->dev
->teimgr
, CHECK_DATA
, skb
);
135 else if (*debug
& DEBUG_SEND_ERR
)
137 "%s ch%d mgr prim(%x) addr(%x) err %d\n",
138 __func__
, ch
->nr
, hh
->prim
, ch
->addr
, ret
);
141 mutex_unlock(&st
->lmutex
);
147 send_msg_to_layer(struct mISDNstack
*st
, struct sk_buff
*skb
)
149 struct mISDNhead
*hh
= mISDN_HEAD_P(skb
);
150 struct mISDNchannel
*ch
;
153 lm
= hh
->prim
& MISDN_LAYERMASK
;
154 if (*debug
& DEBUG_QUEUE_FUNC
)
155 printk(KERN_DEBUG
"%s prim(%x) id(%x) %p\n",
156 __func__
, hh
->prim
, hh
->id
, skb
);
158 if (!hlist_empty(&st
->l1sock
.head
)) {
159 __net_timestamp(skb
);
160 send_socklist(&st
->l1sock
, skb
);
162 return st
->layer1
->send(st
->layer1
, skb
);
163 } else if (lm
== 0x2) {
164 if (!hlist_empty(&st
->l1sock
.head
))
165 send_socklist(&st
->l1sock
, skb
);
166 send_layer2(st
, skb
);
168 } else if (lm
== 0x4) {
169 ch
= get_channel4id(st
, hh
->id
);
171 return ch
->send(ch
, skb
);
174 "%s: dev(%s) prim(%x) id(%x) no channel\n",
175 __func__
, st
->dev
->name
, hh
->prim
, hh
->id
);
176 } else if (lm
== 0x8) {
178 ch
= get_channel4id(st
, hh
->id
);
180 return ch
->send(ch
, skb
);
183 "%s: dev(%s) prim(%x) id(%x) no channel\n",
184 __func__
, st
->dev
->name
, hh
->prim
, hh
->id
);
186 /* broadcast not handled yet */
187 printk(KERN_WARNING
"%s: dev(%s) prim %x not delivered\n",
188 __func__
, st
->dev
->name
, hh
->prim
);
194 do_clear_stack(struct mISDNstack
*st
)
199 mISDNStackd(void *data
)
201 struct mISDNstack
*st
= data
;
207 sigfillset(¤t
->blocked
);
211 if (*debug
& DEBUG_MSG_THREAD
)
212 printk(KERN_DEBUG
"mISDNStackd %s started\n", st
->dev
->name
);
214 if (st
->notify
!= NULL
) {
215 complete(st
->notify
);
222 if (unlikely(test_bit(mISDN_STACK_STOPPED
, &st
->status
))) {
223 test_and_clear_bit(mISDN_STACK_WORK
, &st
->status
);
224 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
226 test_and_set_bit(mISDN_STACK_RUNNING
, &st
->status
);
227 while (test_bit(mISDN_STACK_WORK
, &st
->status
)) {
228 skb
= skb_dequeue(&st
->msgq
);
230 test_and_clear_bit(mISDN_STACK_WORK
,
232 /* test if a race happens */
233 skb
= skb_dequeue(&st
->msgq
);
236 test_and_set_bit(mISDN_STACK_WORK
,
239 #ifdef MISDN_MSG_STATS
242 err
= send_msg_to_layer(st
, skb
);
244 if (*debug
& DEBUG_SEND_ERR
)
246 "%s: %s prim(%x) id(%x) "
248 __func__
, st
->dev
->name
,
249 mISDN_HEAD_PRIM(skb
),
250 mISDN_HEAD_ID(skb
), err
);
254 if (unlikely(test_bit(mISDN_STACK_STOPPED
,
256 test_and_clear_bit(mISDN_STACK_WORK
,
258 test_and_clear_bit(mISDN_STACK_RUNNING
,
263 if (test_bit(mISDN_STACK_CLEARING
, &st
->status
)) {
264 test_and_set_bit(mISDN_STACK_STOPPED
, &st
->status
);
265 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
267 test_and_clear_bit(mISDN_STACK_CLEARING
, &st
->status
);
268 test_and_set_bit(mISDN_STACK_RESTART
, &st
->status
);
270 if (test_and_clear_bit(mISDN_STACK_RESTART
, &st
->status
)) {
271 test_and_clear_bit(mISDN_STACK_STOPPED
, &st
->status
);
272 test_and_set_bit(mISDN_STACK_RUNNING
, &st
->status
);
273 if (!skb_queue_empty(&st
->msgq
))
274 test_and_set_bit(mISDN_STACK_WORK
,
277 if (test_bit(mISDN_STACK_ABORT
, &st
->status
))
279 if (st
->notify
!= NULL
) {
280 complete(st
->notify
);
283 #ifdef MISDN_MSG_STATS
286 test_and_clear_bit(mISDN_STACK_ACTIVE
, &st
->status
);
287 wait_event_interruptible(st
->workq
, (st
->status
&
288 mISDN_STACK_ACTION_MASK
));
289 if (*debug
& DEBUG_MSG_THREAD
)
290 printk(KERN_DEBUG
"%s: %s wake status %08lx\n",
291 __func__
, st
->dev
->name
, st
->status
);
292 test_and_set_bit(mISDN_STACK_ACTIVE
, &st
->status
);
294 test_and_clear_bit(mISDN_STACK_WAKEUP
, &st
->status
);
296 if (test_bit(mISDN_STACK_STOPPED
, &st
->status
)) {
297 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
298 #ifdef MISDN_MSG_STATS
303 #ifdef MISDN_MSG_STATS
304 printk(KERN_DEBUG
"mISDNStackd daemon for %s proceed %d "
305 "msg %d sleep %d stopped\n",
306 st
->dev
->name
, st
->msg_cnt
, st
->sleep_cnt
, st
->stopped_cnt
);
308 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
309 st
->dev
->name
, st
->thread
->utime
, st
->thread
->stime
);
311 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
312 st
->dev
->name
, st
->thread
->nvcsw
, st
->thread
->nivcsw
);
313 printk(KERN_DEBUG
"mISDNStackd daemon for %s killed now\n",
316 test_and_set_bit(mISDN_STACK_KILLED
, &st
->status
);
317 test_and_clear_bit(mISDN_STACK_RUNNING
, &st
->status
);
318 test_and_clear_bit(mISDN_STACK_ACTIVE
, &st
->status
);
319 test_and_clear_bit(mISDN_STACK_ABORT
, &st
->status
);
320 skb_queue_purge(&st
->msgq
);
322 if (st
->notify
!= NULL
) {
323 complete(st
->notify
);
330 l1_receive(struct mISDNchannel
*ch
, struct sk_buff
*skb
)
334 __net_timestamp(skb
);
335 _queue_message(ch
->st
, skb
);
340 set_channel_address(struct mISDNchannel
*ch
, u_int sapi
, u_int tei
)
342 ch
->addr
= sapi
| (tei
<< 8);
346 __add_layer2(struct mISDNchannel
*ch
, struct mISDNstack
*st
)
348 list_add_tail(&ch
->list
, &st
->layer2
);
352 add_layer2(struct mISDNchannel
*ch
, struct mISDNstack
*st
)
354 mutex_lock(&st
->lmutex
);
355 __add_layer2(ch
, st
);
356 mutex_unlock(&st
->lmutex
);
360 st_own_ctrl(struct mISDNchannel
*ch
, u_int cmd
, void *arg
)
362 if (!ch
->st
|| ch
->st
->layer1
)
364 return ch
->st
->layer1
->ctrl(ch
->st
->layer1
, cmd
, arg
);
368 create_stack(struct mISDNdevice
*dev
)
370 struct mISDNstack
*newst
;
372 DECLARE_COMPLETION_ONSTACK(done
);
374 newst
= kzalloc(sizeof(struct mISDNstack
), GFP_KERNEL
);
376 printk(KERN_ERR
"kmalloc mISDN_stack failed\n");
380 INIT_LIST_HEAD(&newst
->layer2
);
381 INIT_HLIST_HEAD(&newst
->l1sock
.head
);
382 rwlock_init(&newst
->l1sock
.lock
);
383 init_waitqueue_head(&newst
->workq
);
384 skb_queue_head_init(&newst
->msgq
);
385 mutex_init(&newst
->lmutex
);
387 err
= create_teimanager(dev
);
389 printk(KERN_ERR
"kmalloc teimanager failed\n");
393 dev
->teimgr
->peer
= &newst
->own
;
394 dev
->teimgr
->recv
= mISDN_queue_message
;
395 dev
->teimgr
->st
= newst
;
396 newst
->layer1
= &dev
->D
;
397 dev
->D
.recv
= l1_receive
;
398 dev
->D
.peer
= &newst
->own
;
399 newst
->own
.st
= newst
;
400 newst
->own
.ctrl
= st_own_ctrl
;
401 newst
->own
.send
= mISDN_queue_message
;
402 newst
->own
.recv
= mISDN_queue_message
;
403 if (*debug
& DEBUG_CORE_FUNC
)
404 printk(KERN_DEBUG
"%s: st(%s)\n", __func__
, newst
->dev
->name
);
405 newst
->notify
= &done
;
406 newst
->thread
= kthread_run(mISDNStackd
, (void *)newst
, "mISDN_%s",
408 if (IS_ERR(newst
->thread
)) {
409 err
= PTR_ERR(newst
->thread
);
411 "mISDN:cannot create kernel thread for %s (%d)\n",
412 newst
->dev
->name
, err
);
413 delete_teimanager(dev
->teimgr
);
416 wait_for_completion(&done
);
421 connect_layer1(struct mISDNdevice
*dev
, struct mISDNchannel
*ch
,
422 u_int protocol
, struct sockaddr_mISDN
*adr
)
424 struct mISDN_sock
*msk
= container_of(ch
, struct mISDN_sock
, ch
);
425 struct channel_req rq
;
429 if (*debug
& DEBUG_CORE_FUNC
)
430 printk(KERN_DEBUG
"%s: %s proto(%x) adr(%d %d %d %d)\n",
431 __func__
, dev
->name
, protocol
, adr
->dev
, adr
->channel
,
432 adr
->sapi
, adr
->tei
);
438 #ifdef PROTOCOL_CHECK
439 /* this should be enhanced */
440 if (!list_empty(&dev
->D
.st
->layer2
)
441 && dev
->D
.protocol
!= protocol
)
443 if (!hlist_empty(&dev
->D
.st
->l1sock
.head
)
444 && dev
->D
.protocol
!= protocol
)
447 ch
->recv
= mISDN_queue_message
;
448 ch
->peer
= &dev
->D
.st
->own
;
450 rq
.protocol
= protocol
;
452 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
453 printk(KERN_DEBUG
"%s: ret 1 %d\n", __func__
, err
);
456 write_lock_bh(&dev
->D
.st
->l1sock
.lock
);
457 sk_add_node(&msk
->sk
, &dev
->D
.st
->l1sock
.head
);
458 write_unlock_bh(&dev
->D
.st
->l1sock
.lock
);
467 connect_Bstack(struct mISDNdevice
*dev
, struct mISDNchannel
*ch
,
468 u_int protocol
, struct sockaddr_mISDN
*adr
)
470 struct channel_req rq
, rq2
;
472 struct Bprotocol
*bp
;
474 if (*debug
& DEBUG_CORE_FUNC
)
475 printk(KERN_DEBUG
"%s: %s proto(%x) adr(%d %d %d %d)\n",
476 __func__
, dev
->name
, protocol
,
477 adr
->dev
, adr
->channel
, adr
->sapi
,
480 pmask
= 1 << (protocol
& ISDN_P_B_MASK
);
481 if (pmask
& dev
->Bprotocols
) {
482 rq
.protocol
= protocol
;
484 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
487 ch
->recv
= rq
.ch
->send
;
489 rq
.ch
->recv
= ch
->send
;
491 rq
.ch
->st
= dev
->D
.st
;
493 bp
= get_Bprotocol4mask(pmask
);
496 rq2
.protocol
= protocol
;
499 err
= bp
->create(&rq2
);
502 ch
->recv
= rq2
.ch
->send
;
504 rq2
.ch
->st
= dev
->D
.st
;
505 rq
.protocol
= rq2
.protocol
;
507 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
509 rq2
.ch
->ctrl(rq2
.ch
, CLOSE_CHANNEL
, NULL
);
512 rq2
.ch
->recv
= rq
.ch
->send
;
513 rq2
.ch
->peer
= rq
.ch
;
514 rq
.ch
->recv
= rq2
.ch
->send
;
515 rq
.ch
->peer
= rq2
.ch
;
516 rq
.ch
->st
= dev
->D
.st
;
518 ch
->protocol
= protocol
;
524 create_l2entity(struct mISDNdevice
*dev
, struct mISDNchannel
*ch
,
525 u_int protocol
, struct sockaddr_mISDN
*adr
)
527 struct channel_req rq
;
530 if (*debug
& DEBUG_CORE_FUNC
)
531 printk(KERN_DEBUG
"%s: %s proto(%x) adr(%d %d %d %d)\n",
532 __func__
, dev
->name
, protocol
,
533 adr
->dev
, adr
->channel
, adr
->sapi
,
535 rq
.protocol
= ISDN_P_TE_S0
;
536 if (dev
->Dprotocols
& (1 << ISDN_P_TE_E1
))
537 rq
.protocol
= ISDN_P_TE_E1
;
540 rq
.protocol
= ISDN_P_NT_S0
;
541 if (dev
->Dprotocols
& (1 << ISDN_P_NT_E1
))
542 rq
.protocol
= ISDN_P_NT_E1
;
544 #ifdef PROTOCOL_CHECK
545 /* this should be enhanced */
546 if (!list_empty(&dev
->D
.st
->layer2
)
547 && dev
->D
.protocol
!= protocol
)
549 if (!hlist_empty(&dev
->D
.st
->l1sock
.head
)
550 && dev
->D
.protocol
!= protocol
)
553 ch
->recv
= mISDN_queue_message
;
554 ch
->peer
= &dev
->D
.st
->own
;
557 err
= dev
->D
.ctrl(&dev
->D
, OPEN_CHANNEL
, &rq
);
558 printk(KERN_DEBUG
"%s: ret 1 %d\n", __func__
, err
);
561 rq
.protocol
= protocol
;
564 err
= dev
->teimgr
->ctrl(dev
->teimgr
, OPEN_CHANNEL
, &rq
);
565 printk(KERN_DEBUG
"%s: ret 2 %d\n", __func__
, err
);
567 if ((protocol
== ISDN_P_LAPD_NT
) && !rq
.ch
)
569 add_layer2(rq
.ch
, dev
->D
.st
);
570 rq
.ch
->recv
= mISDN_queue_message
;
571 rq
.ch
->peer
= &dev
->D
.st
->own
;
572 rq
.ch
->ctrl(rq
.ch
, OPEN_CHANNEL
, NULL
); /* can't fail */
576 err
= -EPROTONOSUPPORT
;
582 delete_channel(struct mISDNchannel
*ch
)
584 struct mISDN_sock
*msk
= container_of(ch
, struct mISDN_sock
, ch
);
585 struct mISDNchannel
*pch
;
588 printk(KERN_WARNING
"%s: no stack\n", __func__
);
591 if (*debug
& DEBUG_CORE_FUNC
)
592 printk(KERN_DEBUG
"%s: st(%s) protocol(%x)\n", __func__
,
593 ch
->st
->dev
->name
, ch
->protocol
);
594 if (ch
->protocol
>= ISDN_P_B_START
) {
596 ch
->peer
->ctrl(ch
->peer
, CLOSE_CHANNEL
, NULL
);
601 switch (ch
->protocol
) {
606 write_lock_bh(&ch
->st
->l1sock
.lock
);
607 sk_del_node_init(&msk
->sk
);
608 write_unlock_bh(&ch
->st
->l1sock
.lock
);
609 ch
->st
->dev
->D
.ctrl(&ch
->st
->dev
->D
, CLOSE_CHANNEL
, NULL
);
612 pch
= get_channel4id(ch
->st
, ch
->nr
);
614 mutex_lock(&ch
->st
->lmutex
);
615 list_del(&pch
->list
);
616 mutex_unlock(&ch
->st
->lmutex
);
617 pch
->ctrl(pch
, CLOSE_CHANNEL
, NULL
);
618 pch
= ch
->st
->dev
->teimgr
;
619 pch
->ctrl(pch
, CLOSE_CHANNEL
, NULL
);
621 printk(KERN_WARNING
"%s: no l2 channel\n",
625 pch
= ch
->st
->dev
->teimgr
;
627 pch
->ctrl(pch
, CLOSE_CHANNEL
, NULL
);
629 printk(KERN_WARNING
"%s: no l2 channel\n",
639 delete_stack(struct mISDNdevice
*dev
)
641 struct mISDNstack
*st
= dev
->D
.st
;
642 DECLARE_COMPLETION_ONSTACK(done
);
644 if (*debug
& DEBUG_CORE_FUNC
)
645 printk(KERN_DEBUG
"%s: st(%s)\n", __func__
,
648 delete_teimanager(dev
->teimgr
);
651 printk(KERN_WARNING
"%s: notifier in use\n",
653 complete(st
->notify
);
656 test_and_set_bit(mISDN_STACK_ABORT
, &st
->status
);
657 test_and_set_bit(mISDN_STACK_WAKEUP
, &st
->status
);
658 wake_up_interruptible(&st
->workq
);
659 wait_for_completion(&done
);
661 if (!list_empty(&st
->layer2
))
662 printk(KERN_WARNING
"%s: layer2 list not empty\n",
664 if (!hlist_empty(&st
->l1sock
.head
))
665 printk(KERN_WARNING
"%s: layer1 list not empty\n",
671 mISDN_initstack(u_int
*dp
)