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>
22 #include <linux/sched/cputime.h>
23 #include <linux/signal.h>
30 _queue_message(struct mISDNstack *st, struct sk_buff *skb)
32 struct mISDNhead *hh = mISDN_HEAD_P(skb);
34 if (*debug & DEBUG_QUEUE_FUNC)
35 printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
36 __func__, hh->prim, hh->id, skb);
37 skb_queue_tail(&st->msgq, skb);
38 if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) {
39 test_and_set_bit(mISDN_STACK_WORK, &st->status);
40 wake_up_interruptible(&st->workq);
45 mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
47 _queue_message(ch->st, skb);
51 static struct mISDNchannel *
52 get_channel4id(struct mISDNstack *st, u_int id)
54 struct mISDNchannel *ch;
56 mutex_lock(&st->lmutex);
57 list_for_each_entry(ch, &st->layer2, list) {
63 mutex_unlock(&st->lmutex);
68 send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
71 struct sk_buff *cskb = NULL;
74 sk_for_each(sk, &sl->head) {
75 if (sk->sk_state != MISDN_BOUND)
78 cskb = skb_copy(skb, GFP_ATOMIC);
80 printk(KERN_WARNING "%s no skb\n", __func__);
83 if (!sock_queue_rcv_skb(sk, cskb))
86 read_unlock(&sl->lock);
92 send_layer2(struct mISDNstack *st, struct sk_buff *skb)
95 struct mISDNhead *hh = mISDN_HEAD_P(skb);
96 struct mISDNchannel *ch;
101 mutex_lock(&st->lmutex);
102 if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) { /* L2 for all */
103 list_for_each_entry(ch, &st->layer2, list) {
104 if (list_is_last(&ch->list, &st->layer2)) {
108 cskb = skb_copy(skb, GFP_KERNEL);
111 ret = ch->send(ch, cskb);
113 if (*debug & DEBUG_SEND_ERR)
115 "%s ch%d prim(%x) addr(%x)"
118 hh->prim, ch->addr, ret);
122 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
123 __func__, ch->nr, ch->addr);
128 list_for_each_entry(ch, &st->layer2, list) {
129 if ((hh->id & MISDN_ID_ADDR_MASK) == ch->addr) {
130 ret = ch->send(ch, skb);
136 ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
139 else if (*debug & DEBUG_SEND_ERR)
141 "%s mgr prim(%x) err %d\n",
142 __func__, hh->prim, ret);
145 mutex_unlock(&st->lmutex);
151 send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
153 struct mISDNhead *hh = mISDN_HEAD_P(skb);
154 struct mISDNchannel *ch;
157 lm = hh->prim & MISDN_LAYERMASK;
158 if (*debug & DEBUG_QUEUE_FUNC)
159 printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
160 __func__, hh->prim, hh->id, skb);
162 if (!hlist_empty(&st->l1sock.head)) {
163 __net_timestamp(skb);
164 send_socklist(&st->l1sock, skb);
166 return st->layer1->send(st->layer1, skb);
167 } else if (lm == 0x2) {
168 if (!hlist_empty(&st->l1sock.head))
169 send_socklist(&st->l1sock, skb);
170 send_layer2(st, skb);
172 } else if (lm == 0x4) {
173 ch = get_channel4id(st, hh->id);
175 return ch->send(ch, skb);
178 "%s: dev(%s) prim(%x) id(%x) no channel\n",
179 __func__, dev_name(&st->dev->dev), hh->prim,
181 } else if (lm == 0x8) {
183 ch = get_channel4id(st, hh->id);
185 return ch->send(ch, skb);
188 "%s: dev(%s) prim(%x) id(%x) no channel\n",
189 __func__, dev_name(&st->dev->dev), hh->prim,
192 /* broadcast not handled yet */
193 printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
194 __func__, dev_name(&st->dev->dev), hh->prim);
200 do_clear_stack(struct mISDNstack *st)
205 mISDNStackd(void *data)
207 struct mISDNstack *st = data;
208 #ifdef MISDN_MSG_STATS
213 sigfillset(¤t->blocked);
214 if (*debug & DEBUG_MSG_THREAD)
215 printk(KERN_DEBUG "mISDNStackd %s started\n",
216 dev_name(&st->dev->dev));
218 if (st->notify != NULL) {
219 complete(st->notify);
226 if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
227 test_and_clear_bit(mISDN_STACK_WORK, &st->status);
228 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
230 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
231 while (test_bit(mISDN_STACK_WORK, &st->status)) {
232 skb = skb_dequeue(&st->msgq);
234 test_and_clear_bit(mISDN_STACK_WORK,
236 /* test if a race happens */
237 skb = skb_dequeue(&st->msgq);
240 test_and_set_bit(mISDN_STACK_WORK,
243 #ifdef MISDN_MSG_STATS
246 err = send_msg_to_layer(st, skb);
248 if (*debug & DEBUG_SEND_ERR)
250 "%s: %s prim(%x) id(%x) "
252 __func__, dev_name(&st->dev->dev),
253 mISDN_HEAD_PRIM(skb),
254 mISDN_HEAD_ID(skb), err);
258 if (unlikely(test_bit(mISDN_STACK_STOPPED,
260 test_and_clear_bit(mISDN_STACK_WORK,
262 test_and_clear_bit(mISDN_STACK_RUNNING,
267 if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
268 test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
269 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
271 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
272 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
274 if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
275 test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
276 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
277 if (!skb_queue_empty(&st->msgq))
278 test_and_set_bit(mISDN_STACK_WORK,
281 if (test_bit(mISDN_STACK_ABORT, &st->status))
283 if (st->notify != NULL) {
284 complete(st->notify);
287 #ifdef MISDN_MSG_STATS
290 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
291 wait_event_interruptible(st->workq, (st->status &
292 mISDN_STACK_ACTION_MASK));
293 if (*debug & DEBUG_MSG_THREAD)
294 printk(KERN_DEBUG "%s: %s wake status %08lx\n",
295 __func__, dev_name(&st->dev->dev), st->status);
296 test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
298 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
300 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
301 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
302 #ifdef MISDN_MSG_STATS
307 #ifdef MISDN_MSG_STATS
308 printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
309 "msg %d sleep %d stopped\n",
310 dev_name(&st->dev->dev), st->msg_cnt, st->sleep_cnt,
312 task_cputime(st->thread, &utime, &stime);
314 "mISDNStackd daemon for %s utime(%llu) stime(%llu)\n",
315 dev_name(&st->dev->dev), utime, stime);
317 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
318 dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
319 printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
320 dev_name(&st->dev->dev));
322 test_and_set_bit(mISDN_STACK_KILLED, &st->status);
323 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
324 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
325 test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
326 skb_queue_purge(&st->msgq);
328 if (st->notify != NULL) {
329 complete(st->notify);
336 l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
340 __net_timestamp(skb);
341 _queue_message(ch->st, skb);
346 set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
348 ch->addr = sapi | (tei << 8);
352 __add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
354 list_add_tail(&ch->list, &st->layer2);
358 add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
360 mutex_lock(&st->lmutex);
361 __add_layer2(ch, st);
362 mutex_unlock(&st->lmutex);
366 st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
368 if (!ch->st || !ch->st->layer1)
370 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
374 create_stack(struct mISDNdevice *dev)
376 struct mISDNstack *newst;
378 DECLARE_COMPLETION_ONSTACK(done);
380 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
382 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
386 INIT_LIST_HEAD(&newst->layer2);
387 INIT_HLIST_HEAD(&newst->l1sock.head);
388 rwlock_init(&newst->l1sock.lock);
389 init_waitqueue_head(&newst->workq);
390 skb_queue_head_init(&newst->msgq);
391 mutex_init(&newst->lmutex);
393 err = create_teimanager(dev);
395 printk(KERN_ERR "kmalloc teimanager failed\n");
399 dev->teimgr->peer = &newst->own;
400 dev->teimgr->recv = mISDN_queue_message;
401 dev->teimgr->st = newst;
402 newst->layer1 = &dev->D;
403 dev->D.recv = l1_receive;
404 dev->D.peer = &newst->own;
405 newst->own.st = newst;
406 newst->own.ctrl = st_own_ctrl;
407 newst->own.send = mISDN_queue_message;
408 newst->own.recv = mISDN_queue_message;
409 if (*debug & DEBUG_CORE_FUNC)
410 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
411 dev_name(&newst->dev->dev));
412 newst->notify = &done;
413 newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
414 dev_name(&newst->dev->dev));
415 if (IS_ERR(newst->thread)) {
416 err = PTR_ERR(newst->thread);
418 "mISDN:cannot create kernel thread for %s (%d)\n",
419 dev_name(&newst->dev->dev), err);
420 delete_teimanager(dev->teimgr);
423 wait_for_completion(&done);
428 connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
429 u_int protocol, struct sockaddr_mISDN *adr)
431 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
432 struct channel_req rq;
436 if (*debug & DEBUG_CORE_FUNC)
437 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
438 __func__, dev_name(&dev->dev), protocol, adr->dev,
439 adr->channel, adr->sapi, adr->tei);
445 ch->recv = mISDN_queue_message;
446 ch->peer = &dev->D.st->own;
448 rq.protocol = protocol;
449 rq.adr.channel = adr->channel;
450 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
451 printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
455 write_lock_bh(&dev->D.st->l1sock.lock);
456 sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
457 write_unlock_bh(&dev->D.st->l1sock.lock);
466 connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
467 u_int protocol, struct sockaddr_mISDN *adr)
469 struct channel_req rq, rq2;
471 struct Bprotocol *bp;
473 if (*debug & DEBUG_CORE_FUNC)
474 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
475 __func__, dev_name(&dev->dev), protocol,
476 adr->dev, adr->channel, adr->sapi,
479 pmask = 1 << (protocol & ISDN_P_B_MASK);
480 if (pmask & dev->Bprotocols) {
481 rq.protocol = protocol;
483 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
486 ch->recv = rq.ch->send;
488 rq.ch->recv = ch->send;
490 rq.ch->st = dev->D.st;
492 bp = get_Bprotocol4mask(pmask);
495 rq2.protocol = protocol;
498 err = bp->create(&rq2);
501 ch->recv = rq2.ch->send;
503 rq2.ch->st = dev->D.st;
504 rq.protocol = rq2.protocol;
506 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
508 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
511 rq2.ch->recv = rq.ch->send;
512 rq2.ch->peer = rq.ch;
513 rq.ch->recv = rq2.ch->send;
514 rq.ch->peer = rq2.ch;
515 rq.ch->st = dev->D.st;
517 ch->protocol = protocol;
523 create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
524 u_int protocol, struct sockaddr_mISDN *adr)
526 struct channel_req rq;
529 if (*debug & DEBUG_CORE_FUNC)
530 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
531 __func__, dev_name(&dev->dev), protocol,
532 adr->dev, adr->channel, adr->sapi,
534 rq.protocol = ISDN_P_TE_S0;
535 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
536 rq.protocol = ISDN_P_TE_E1;
539 rq.protocol = ISDN_P_NT_S0;
540 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
541 rq.protocol = ISDN_P_NT_E1;
544 ch->recv = mISDN_queue_message;
545 ch->peer = &dev->D.st->own;
548 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
549 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
552 rq.protocol = protocol;
555 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
556 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
558 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
560 add_layer2(rq.ch, dev->D.st);
561 rq.ch->recv = mISDN_queue_message;
562 rq.ch->peer = &dev->D.st->own;
563 rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
567 err = -EPROTONOSUPPORT;
573 delete_channel(struct mISDNchannel *ch)
575 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
576 struct mISDNchannel *pch;
579 printk(KERN_WARNING "%s: no stack\n", __func__);
582 if (*debug & DEBUG_CORE_FUNC)
583 printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
584 dev_name(&ch->st->dev->dev), ch->protocol);
585 if (ch->protocol >= ISDN_P_B_START) {
587 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
592 switch (ch->protocol) {
597 write_lock_bh(&ch->st->l1sock.lock);
598 sk_del_node_init(&msk->sk);
599 write_unlock_bh(&ch->st->l1sock.lock);
600 ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
603 pch = get_channel4id(ch->st, ch->nr);
605 mutex_lock(&ch->st->lmutex);
606 list_del(&pch->list);
607 mutex_unlock(&ch->st->lmutex);
608 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
609 pch = ch->st->dev->teimgr;
610 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
612 printk(KERN_WARNING "%s: no l2 channel\n",
616 pch = ch->st->dev->teimgr;
618 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
620 printk(KERN_WARNING "%s: no l2 channel\n",
630 delete_stack(struct mISDNdevice *dev)
632 struct mISDNstack *st = dev->D.st;
633 DECLARE_COMPLETION_ONSTACK(done);
635 if (*debug & DEBUG_CORE_FUNC)
636 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
637 dev_name(&st->dev->dev));
639 delete_teimanager(dev->teimgr);
642 printk(KERN_WARNING "%s: notifier in use\n",
644 complete(st->notify);
647 test_and_set_bit(mISDN_STACK_ABORT, &st->status);
648 test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
649 wake_up_interruptible(&st->workq);
650 wait_for_completion(&done);
652 if (!list_empty(&st->layer2))
653 printk(KERN_WARNING "%s: layer2 list not empty\n",
655 if (!hlist_empty(&st->l1sock.head))
656 printk(KERN_WARNING "%s: layer1 list not empty\n",
662 mISDN_initstack(u_int *dp)