Merge tag 'v3.3.7' into 3.3/master
[zen-stable.git] / drivers / isdn / mISDN / stack.c
bloba5b632e6755264bb33205f55cb62fc85a1f42b43
1 /*
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 "core.h"
23 static u_int *debug;
25 static inline void
26 _queue_message(struct mISDNstack *st, struct sk_buff *skb)
28 struct mISDNhead *hh = mISDN_HEAD_P(skb);
30 if (*debug & DEBUG_QUEUE_FUNC)
31 printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
32 __func__, hh->prim, hh->id, skb);
33 skb_queue_tail(&st->msgq, skb);
34 if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) {
35 test_and_set_bit(mISDN_STACK_WORK, &st->status);
36 wake_up_interruptible(&st->workq);
40 static int
41 mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
43 _queue_message(ch->st, skb);
44 return 0;
47 static struct mISDNchannel *
48 get_channel4id(struct mISDNstack *st, u_int id)
50 struct mISDNchannel *ch;
52 mutex_lock(&st->lmutex);
53 list_for_each_entry(ch, &st->layer2, list) {
54 if (id == ch->nr)
55 goto unlock;
57 ch = NULL;
58 unlock:
59 mutex_unlock(&st->lmutex);
60 return ch;
63 static void
64 send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
66 struct hlist_node *node;
67 struct sock *sk;
68 struct sk_buff *cskb = NULL;
70 read_lock(&sl->lock);
71 sk_for_each(sk, node, &sl->head) {
72 if (sk->sk_state != MISDN_BOUND)
73 continue;
74 if (!cskb)
75 cskb = skb_copy(skb, GFP_KERNEL);
76 if (!cskb) {
77 printk(KERN_WARNING "%s no skb\n", __func__);
78 break;
80 if (!sock_queue_rcv_skb(sk, cskb))
81 cskb = NULL;
83 read_unlock(&sl->lock);
84 if (cskb)
85 dev_kfree_skb(cskb);
88 static void
89 send_layer2(struct mISDNstack *st, struct sk_buff *skb)
91 struct sk_buff *cskb;
92 struct mISDNhead *hh = mISDN_HEAD_P(skb);
93 struct mISDNchannel *ch;
94 int ret;
96 if (!st)
97 return;
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)) {
102 cskb = skb;
103 skb = NULL;
104 } else {
105 cskb = skb_copy(skb, GFP_KERNEL);
107 if (cskb) {
108 ret = ch->send(ch, cskb);
109 if (ret) {
110 if (*debug & DEBUG_SEND_ERR)
111 printk(KERN_DEBUG
112 "%s ch%d prim(%x) addr(%x)"
113 " err %d\n",
114 __func__, ch->nr,
115 hh->prim, ch->addr, ret);
116 dev_kfree_skb(cskb);
118 } else {
119 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
120 __func__, ch->nr, ch->addr);
121 goto out;
124 } else {
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);
128 if (!ret)
129 skb = NULL;
130 goto out;
133 ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
134 if (!ret)
135 skb = NULL;
136 else if (*debug & DEBUG_SEND_ERR)
137 printk(KERN_DEBUG
138 "%s ch%d mgr prim(%x) addr(%x) err %d\n",
139 __func__, ch->nr, hh->prim, ch->addr, ret);
141 out:
142 mutex_unlock(&st->lmutex);
143 if (skb)
144 dev_kfree_skb(skb);
147 static inline int
148 send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
150 struct mISDNhead *hh = mISDN_HEAD_P(skb);
151 struct mISDNchannel *ch;
152 int lm;
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);
158 if (lm == 0x1) {
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);
168 return 0;
169 } else if (lm == 0x4) {
170 ch = get_channel4id(st, hh->id);
171 if (ch)
172 return ch->send(ch, skb);
173 else
174 printk(KERN_WARNING
175 "%s: dev(%s) prim(%x) id(%x) no channel\n",
176 __func__, dev_name(&st->dev->dev), hh->prim,
177 hh->id);
178 } else if (lm == 0x8) {
179 WARN_ON(lm == 0x8);
180 ch = get_channel4id(st, hh->id);
181 if (ch)
182 return ch->send(ch, skb);
183 else
184 printk(KERN_WARNING
185 "%s: dev(%s) prim(%x) id(%x) no channel\n",
186 __func__, dev_name(&st->dev->dev), hh->prim,
187 hh->id);
188 } else {
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);
193 return -ESRCH;
196 static void
197 do_clear_stack(struct mISDNstack *st)
201 static int
202 mISDNStackd(void *data)
204 struct mISDNstack *st = data;
205 int err = 0;
207 sigfillset(&current->blocked);
208 if (*debug & DEBUG_MSG_THREAD)
209 printk(KERN_DEBUG "mISDNStackd %s started\n",
210 dev_name(&st->dev->dev));
212 if (st->notify != NULL) {
213 complete(st->notify);
214 st->notify = NULL;
217 for (;;) {
218 struct sk_buff *skb;
220 if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
221 test_and_clear_bit(mISDN_STACK_WORK, &st->status);
222 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
223 } else
224 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
225 while (test_bit(mISDN_STACK_WORK, &st->status)) {
226 skb = skb_dequeue(&st->msgq);
227 if (!skb) {
228 test_and_clear_bit(mISDN_STACK_WORK,
229 &st->status);
230 /* test if a race happens */
231 skb = skb_dequeue(&st->msgq);
232 if (!skb)
233 continue;
234 test_and_set_bit(mISDN_STACK_WORK,
235 &st->status);
237 #ifdef MISDN_MSG_STATS
238 st->msg_cnt++;
239 #endif
240 err = send_msg_to_layer(st, skb);
241 if (unlikely(err)) {
242 if (*debug & DEBUG_SEND_ERR)
243 printk(KERN_DEBUG
244 "%s: %s prim(%x) id(%x) "
245 "send call(%d)\n",
246 __func__, dev_name(&st->dev->dev),
247 mISDN_HEAD_PRIM(skb),
248 mISDN_HEAD_ID(skb), err);
249 dev_kfree_skb(skb);
250 continue;
252 if (unlikely(test_bit(mISDN_STACK_STOPPED,
253 &st->status))) {
254 test_and_clear_bit(mISDN_STACK_WORK,
255 &st->status);
256 test_and_clear_bit(mISDN_STACK_RUNNING,
257 &st->status);
258 break;
261 if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
262 test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
263 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
264 do_clear_stack(st);
265 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
266 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
268 if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
269 test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
270 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
271 if (!skb_queue_empty(&st->msgq))
272 test_and_set_bit(mISDN_STACK_WORK,
273 &st->status);
275 if (test_bit(mISDN_STACK_ABORT, &st->status))
276 break;
277 if (st->notify != NULL) {
278 complete(st->notify);
279 st->notify = NULL;
281 #ifdef MISDN_MSG_STATS
282 st->sleep_cnt++;
283 #endif
284 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
285 wait_event_interruptible(st->workq, (st->status &
286 mISDN_STACK_ACTION_MASK));
287 if (*debug & DEBUG_MSG_THREAD)
288 printk(KERN_DEBUG "%s: %s wake status %08lx\n",
289 __func__, dev_name(&st->dev->dev), st->status);
290 test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
292 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
294 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
295 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
296 #ifdef MISDN_MSG_STATS
297 st->stopped_cnt++;
298 #endif
301 #ifdef MISDN_MSG_STATS
302 printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
303 "msg %d sleep %d stopped\n",
304 dev_name(&st->dev->dev), st->msg_cnt, st->sleep_cnt,
305 st->stopped_cnt);
306 printk(KERN_DEBUG
307 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
308 dev_name(&st->dev->dev), st->thread->utime, st->thread->stime);
309 printk(KERN_DEBUG
310 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
311 dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
312 printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
313 dev_name(&st->dev->dev));
314 #endif
315 test_and_set_bit(mISDN_STACK_KILLED, &st->status);
316 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
317 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
318 test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
319 skb_queue_purge(&st->msgq);
320 st->thread = NULL;
321 if (st->notify != NULL) {
322 complete(st->notify);
323 st->notify = NULL;
325 return 0;
328 static int
329 l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
331 if (!ch->st)
332 return -ENODEV;
333 __net_timestamp(skb);
334 _queue_message(ch->st, skb);
335 return 0;
338 void
339 set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
341 ch->addr = sapi | (tei << 8);
344 void
345 __add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
347 list_add_tail(&ch->list, &st->layer2);
350 void
351 add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
353 mutex_lock(&st->lmutex);
354 __add_layer2(ch, st);
355 mutex_unlock(&st->lmutex);
358 static int
359 st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
361 if (!ch->st || !ch->st->layer1)
362 return -EINVAL;
363 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
367 create_stack(struct mISDNdevice *dev)
369 struct mISDNstack *newst;
370 int err;
371 DECLARE_COMPLETION_ONSTACK(done);
373 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
374 if (!newst) {
375 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
376 return -ENOMEM;
378 newst->dev = dev;
379 INIT_LIST_HEAD(&newst->layer2);
380 INIT_HLIST_HEAD(&newst->l1sock.head);
381 rwlock_init(&newst->l1sock.lock);
382 init_waitqueue_head(&newst->workq);
383 skb_queue_head_init(&newst->msgq);
384 mutex_init(&newst->lmutex);
385 dev->D.st = newst;
386 err = create_teimanager(dev);
387 if (err) {
388 printk(KERN_ERR "kmalloc teimanager failed\n");
389 kfree(newst);
390 return err;
392 dev->teimgr->peer = &newst->own;
393 dev->teimgr->recv = mISDN_queue_message;
394 dev->teimgr->st = newst;
395 newst->layer1 = &dev->D;
396 dev->D.recv = l1_receive;
397 dev->D.peer = &newst->own;
398 newst->own.st = newst;
399 newst->own.ctrl = st_own_ctrl;
400 newst->own.send = mISDN_queue_message;
401 newst->own.recv = mISDN_queue_message;
402 if (*debug & DEBUG_CORE_FUNC)
403 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
404 dev_name(&newst->dev->dev));
405 newst->notify = &done;
406 newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
407 dev_name(&newst->dev->dev));
408 if (IS_ERR(newst->thread)) {
409 err = PTR_ERR(newst->thread);
410 printk(KERN_ERR
411 "mISDN:cannot create kernel thread for %s (%d)\n",
412 dev_name(&newst->dev->dev), err);
413 delete_teimanager(dev->teimgr);
414 kfree(newst);
415 } else
416 wait_for_completion(&done);
417 return err;
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;
426 int err;
429 if (*debug & DEBUG_CORE_FUNC)
430 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
431 __func__, dev_name(&dev->dev), protocol, adr->dev,
432 adr->channel, adr->sapi, adr->tei);
433 switch (protocol) {
434 case ISDN_P_NT_S0:
435 case ISDN_P_NT_E1:
436 case ISDN_P_TE_S0:
437 case ISDN_P_TE_E1:
438 ch->recv = mISDN_queue_message;
439 ch->peer = &dev->D.st->own;
440 ch->st = dev->D.st;
441 rq.protocol = protocol;
442 rq.adr.channel = adr->channel;
443 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
444 printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
445 dev->id);
446 if (err)
447 return err;
448 write_lock_bh(&dev->D.st->l1sock.lock);
449 sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
450 write_unlock_bh(&dev->D.st->l1sock.lock);
451 break;
452 default:
453 return -ENOPROTOOPT;
455 return 0;
459 connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
460 u_int protocol, struct sockaddr_mISDN *adr)
462 struct channel_req rq, rq2;
463 int pmask, err;
464 struct Bprotocol *bp;
466 if (*debug & DEBUG_CORE_FUNC)
467 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
468 __func__, dev_name(&dev->dev), protocol,
469 adr->dev, adr->channel, adr->sapi,
470 adr->tei);
471 ch->st = dev->D.st;
472 pmask = 1 << (protocol & ISDN_P_B_MASK);
473 if (pmask & dev->Bprotocols) {
474 rq.protocol = protocol;
475 rq.adr = *adr;
476 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
477 if (err)
478 return err;
479 ch->recv = rq.ch->send;
480 ch->peer = rq.ch;
481 rq.ch->recv = ch->send;
482 rq.ch->peer = ch;
483 rq.ch->st = dev->D.st;
484 } else {
485 bp = get_Bprotocol4mask(pmask);
486 if (!bp)
487 return -ENOPROTOOPT;
488 rq2.protocol = protocol;
489 rq2.adr = *adr;
490 rq2.ch = ch;
491 err = bp->create(&rq2);
492 if (err)
493 return err;
494 ch->recv = rq2.ch->send;
495 ch->peer = rq2.ch;
496 rq2.ch->st = dev->D.st;
497 rq.protocol = rq2.protocol;
498 rq.adr = *adr;
499 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
500 if (err) {
501 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
502 return err;
504 rq2.ch->recv = rq.ch->send;
505 rq2.ch->peer = rq.ch;
506 rq.ch->recv = rq2.ch->send;
507 rq.ch->peer = rq2.ch;
508 rq.ch->st = dev->D.st;
510 ch->protocol = protocol;
511 ch->nr = rq.ch->nr;
512 return 0;
516 create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
517 u_int protocol, struct sockaddr_mISDN *adr)
519 struct channel_req rq;
520 int err;
522 if (*debug & DEBUG_CORE_FUNC)
523 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
524 __func__, dev_name(&dev->dev), protocol,
525 adr->dev, adr->channel, adr->sapi,
526 adr->tei);
527 rq.protocol = ISDN_P_TE_S0;
528 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
529 rq.protocol = ISDN_P_TE_E1;
530 switch (protocol) {
531 case ISDN_P_LAPD_NT:
532 rq.protocol = ISDN_P_NT_S0;
533 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
534 rq.protocol = ISDN_P_NT_E1;
535 case ISDN_P_LAPD_TE:
536 ch->recv = mISDN_queue_message;
537 ch->peer = &dev->D.st->own;
538 ch->st = dev->D.st;
539 rq.adr.channel = 0;
540 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
541 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
542 if (err)
543 break;
544 rq.protocol = protocol;
545 rq.adr = *adr;
546 rq.ch = ch;
547 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
548 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
549 if (!err) {
550 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
551 break;
552 add_layer2(rq.ch, dev->D.st);
553 rq.ch->recv = mISDN_queue_message;
554 rq.ch->peer = &dev->D.st->own;
555 rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
557 break;
558 default:
559 err = -EPROTONOSUPPORT;
561 return err;
564 void
565 delete_channel(struct mISDNchannel *ch)
567 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
568 struct mISDNchannel *pch;
570 if (!ch->st) {
571 printk(KERN_WARNING "%s: no stack\n", __func__);
572 return;
574 if (*debug & DEBUG_CORE_FUNC)
575 printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
576 dev_name(&ch->st->dev->dev), ch->protocol);
577 if (ch->protocol >= ISDN_P_B_START) {
578 if (ch->peer) {
579 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
580 ch->peer = NULL;
582 return;
584 switch (ch->protocol) {
585 case ISDN_P_NT_S0:
586 case ISDN_P_TE_S0:
587 case ISDN_P_NT_E1:
588 case ISDN_P_TE_E1:
589 write_lock_bh(&ch->st->l1sock.lock);
590 sk_del_node_init(&msk->sk);
591 write_unlock_bh(&ch->st->l1sock.lock);
592 ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
593 break;
594 case ISDN_P_LAPD_TE:
595 pch = get_channel4id(ch->st, ch->nr);
596 if (pch) {
597 mutex_lock(&ch->st->lmutex);
598 list_del(&pch->list);
599 mutex_unlock(&ch->st->lmutex);
600 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
601 pch = ch->st->dev->teimgr;
602 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
603 } else
604 printk(KERN_WARNING "%s: no l2 channel\n",
605 __func__);
606 break;
607 case ISDN_P_LAPD_NT:
608 pch = ch->st->dev->teimgr;
609 if (pch) {
610 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
611 } else
612 printk(KERN_WARNING "%s: no l2 channel\n",
613 __func__);
614 break;
615 default:
616 break;
618 return;
621 void
622 delete_stack(struct mISDNdevice *dev)
624 struct mISDNstack *st = dev->D.st;
625 DECLARE_COMPLETION_ONSTACK(done);
627 if (*debug & DEBUG_CORE_FUNC)
628 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
629 dev_name(&st->dev->dev));
630 if (dev->teimgr)
631 delete_teimanager(dev->teimgr);
632 if (st->thread) {
633 if (st->notify) {
634 printk(KERN_WARNING "%s: notifier in use\n",
635 __func__);
636 complete(st->notify);
638 st->notify = &done;
639 test_and_set_bit(mISDN_STACK_ABORT, &st->status);
640 test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
641 wake_up_interruptible(&st->workq);
642 wait_for_completion(&done);
644 if (!list_empty(&st->layer2))
645 printk(KERN_WARNING "%s: layer2 list not empty\n",
646 __func__);
647 if (!hlist_empty(&st->l1sock.head))
648 printk(KERN_WARNING "%s: layer1 list not empty\n",
649 __func__);
650 kfree(st);
653 void
654 mISDN_initstack(u_int *dp)
656 debug = dp;