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)