1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18#include <linux/mISDNif.h>
19#include <linux/kthread.h>
20#include "core.h"
21
22static u_int *debug;
23
24static inline void
25_queue_message(struct mISDNstack *st, struct sk_buff *skb)
26{
27 struct mISDNhead *hh = mISDN_HEAD_P(skb);
28
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);
36 }
37}
38
39int
40mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
41{
42 _queue_message(ch->st, skb);
43 return 0;
44}
45
46static struct mISDNchannel *
47get_channel4id(struct mISDNstack *st, u_int id)
48{
49 struct mISDNchannel *ch;
50
51 mutex_lock(&st->lmutex);
52 list_for_each_entry(ch, &st->layer2, list) {
53 if (id == ch->nr)
54 goto unlock;
55 }
56 ch = NULL;
57unlock:
58 mutex_unlock(&st->lmutex);
59 return ch;
60}
61
62static void
63send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
64{
65 struct hlist_node *node;
66 struct sock *sk;
67 struct sk_buff *cskb = NULL;
68
69 read_lock(&sl->lock);
70 sk_for_each(sk, node, &sl->head) {
71 if (sk->sk_state != MISDN_BOUND)
72 continue;
73 if (!cskb)
74 cskb = skb_copy(skb, GFP_KERNEL);
75 if (!cskb) {
76 printk(KERN_WARNING "%s no skb\n", __func__);
77 break;
78 }
79 if (!sock_queue_rcv_skb(sk, cskb))
80 cskb = NULL;
81 }
82 read_unlock(&sl->lock);
83 if (cskb)
84 dev_kfree_skb(cskb);
85}
86
87static void
88send_layer2(struct mISDNstack *st, struct sk_buff *skb)
89{
90 struct sk_buff *cskb;
91 struct mISDNhead *hh = mISDN_HEAD_P(skb);
92 struct mISDNchannel *ch;
93 int ret;
94
95 if (!st)
96 return;
97 mutex_lock(&st->lmutex);
98 if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) {
99 list_for_each_entry(ch, &st->layer2, list) {
100 if (list_is_last(&ch->list, &st->layer2)) {
101 cskb = skb;
102 skb = NULL;
103 } else {
104 cskb = skb_copy(skb, GFP_KERNEL);
105 }
106 if (cskb) {
107 ret = ch->send(ch, cskb);
108 if (ret) {
109 if (*debug & DEBUG_SEND_ERR)
110 printk(KERN_DEBUG
111 "%s ch%d prim(%x) addr(%x)"
112 " err %d\n",
113 __func__, ch->nr,
114 hh->prim, ch->addr, ret);
115 dev_kfree_skb(cskb);
116 }
117 } else {
118 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
119 __func__, ch->nr, ch->addr);
120 goto out;
121 }
122 }
123 } else {
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);
127 if (!ret)
128 skb = NULL;
129 goto out;
130 }
131 }
132 ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
133 if (!ret)
134 skb = NULL;
135 else if (*debug & DEBUG_SEND_ERR)
136 printk(KERN_DEBUG
137 "%s ch%d mgr prim(%x) addr(%x) err %d\n",
138 __func__, ch->nr, hh->prim, ch->addr, ret);
139 }
140out:
141 mutex_unlock(&st->lmutex);
142 if (skb)
143 dev_kfree_skb(skb);
144}
145
146static inline int
147send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
148{
149 struct mISDNhead *hh = mISDN_HEAD_P(skb);
150 struct mISDNchannel *ch;
151 int lm;
152
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);
157 if (lm == 0x1) {
158 if (!hlist_empty(&st->l1sock.head)) {
159 __net_timestamp(skb);
160 send_socklist(&st->l1sock, skb);
161 }
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);
167 return 0;
168 } else if (lm == 0x4) {
169 ch = get_channel4id(st, hh->id);
170 if (ch)
171 return ch->send(ch, skb);
172 else
173 printk(KERN_WARNING
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) {
177 WARN_ON(lm == 0x8);
178 ch = get_channel4id(st, hh->id);
179 if (ch)
180 return ch->send(ch, skb);
181 else
182 printk(KERN_WARNING
183 "%s: dev(%s) prim(%x) id(%x) no channel\n",
184 __func__, st->dev->name, hh->prim, hh->id);
185 } else {
186
187 printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
188 __func__, st->dev->name, hh->prim);
189 }
190 return -ESRCH;
191}
192
193static void
194do_clear_stack(struct mISDNstack *st)
195{
196}
197
198static int
199mISDNStackd(void *data)
200{
201 struct mISDNstack *st = data;
202 int err = 0;
203
204#ifdef CONFIG_SMP
205 lock_kernel();
206#endif
207 sigfillset(¤t->blocked);
208#ifdef CONFIG_SMP
209 unlock_kernel();
210#endif
211 if (*debug & DEBUG_MSG_THREAD)
212 printk(KERN_DEBUG "mISDNStackd %s started\n", st->dev->name);
213
214 if (st->notify != NULL) {
215 complete(st->notify);
216 st->notify = NULL;
217 }
218
219 for (;;) {
220 struct sk_buff *skb;
221
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);
225 } else
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);
229 if (!skb) {
230 test_and_clear_bit(mISDN_STACK_WORK,
231 &st->status);
232
233 skb = skb_dequeue(&st->msgq);
234 if (!skb)
235 continue;
236 test_and_set_bit(mISDN_STACK_WORK,
237 &st->status);
238 }
239#ifdef MISDN_MSG_STATS
240 st->msg_cnt++;
241#endif
242 err = send_msg_to_layer(st, skb);
243 if (unlikely(err)) {
244 if (*debug & DEBUG_SEND_ERR)
245 printk(KERN_DEBUG
246 "%s: %s prim(%x) id(%x) "
247 "send call(%d)\n",
248 __func__, st->dev->name,
249 mISDN_HEAD_PRIM(skb),
250 mISDN_HEAD_ID(skb), err);
251 dev_kfree_skb(skb);
252 continue;
253 }
254 if (unlikely(test_bit(mISDN_STACK_STOPPED,
255 &st->status))) {
256 test_and_clear_bit(mISDN_STACK_WORK,
257 &st->status);
258 test_and_clear_bit(mISDN_STACK_RUNNING,
259 &st->status);
260 break;
261 }
262 }
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);
266 do_clear_stack(st);
267 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
268 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
269 }
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,
275 &st->status);
276 }
277 if (test_bit(mISDN_STACK_ABORT, &st->status))
278 break;
279 if (st->notify != NULL) {
280 complete(st->notify);
281 st->notify = NULL;
282 }
283#ifdef MISDN_MSG_STATS
284 st->sleep_cnt++;
285#endif
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);
293
294 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
295
296 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
297 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
298#ifdef MISDN_MSG_STATS
299 st->stopped_cnt++;
300#endif
301 }
302 }
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);
307 printk(KERN_DEBUG
308 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
309 st->dev->name, st->thread->utime, st->thread->stime);
310 printk(KERN_DEBUG
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",
314 st->dev->name);
315#endif
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);
321 st->thread = NULL;
322 if (st->notify != NULL) {
323 complete(st->notify);
324 st->notify = NULL;
325 }
326 return 0;
327}
328
329static int
330l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
331{
332 if (!ch->st)
333 return -ENODEV;
334 __net_timestamp(skb);
335 _queue_message(ch->st, skb);
336 return 0;
337}
338
339void
340set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
341{
342 ch->addr = sapi | (tei << 8);
343}
344
345void
346__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
347{
348 list_add_tail(&ch->list, &st->layer2);
349}
350
351void
352add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
353{
354 mutex_lock(&st->lmutex);
355 __add_layer2(ch, st);
356 mutex_unlock(&st->lmutex);
357}
358
359static int
360st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
361{
362 if (!ch->st || ch->st->layer1)
363 return -EINVAL;
364 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
365}
366
367int
368create_stack(struct mISDNdevice *dev)
369{
370 struct mISDNstack *newst;
371 int err;
372 DECLARE_COMPLETION_ONSTACK(done);
373
374 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
375 if (!newst) {
376 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
377 return -ENOMEM;
378 }
379 newst->dev = dev;
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);
386 dev->D.st = newst;
387 err = create_teimanager(dev);
388 if (err) {
389 printk(KERN_ERR "kmalloc teimanager failed\n");
390 kfree(newst);
391 return err;
392 }
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",
407 newst->dev->name);
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 newst->dev->name, err);
413 delete_teimanager(dev->teimgr);
414 kfree(newst);
415 } else
416 wait_for_completion(&done);
417 return err;
418}
419
420int
421connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
422 u_int protocol, struct sockaddr_mISDN *adr)
423{
424 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
425 struct channel_req rq;
426 int err;
427
428
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);
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#ifdef PROTOCOL_CHECK
439
440 if (!list_empty(&dev->D.st->layer2)
441 && dev->D.protocol != protocol)
442 return -EBUSY;
443 if (!hlist_empty(&dev->D.st->l1sock.head)
444 && dev->D.protocol != protocol)
445 return -EBUSY;
446#endif
447 ch->recv = mISDN_queue_message;
448 ch->peer = &dev->D.st->own;
449 ch->st = dev->D.st;
450 rq.protocol = protocol;
451 rq.adr.channel = 0;
452 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
453 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
454 if (err)
455 return 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);
459 break;
460 default:
461 return -ENOPROTOOPT;
462 }
463 return 0;
464}
465
466int
467connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
468 u_int protocol, struct sockaddr_mISDN *adr)
469{
470 struct channel_req rq, rq2;
471 int pmask, err;
472 struct Bprotocol *bp;
473
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,
478 adr->tei);
479 ch->st = dev->D.st;
480 pmask = 1 << (protocol & ISDN_P_B_MASK);
481 if (pmask & dev->Bprotocols) {
482 rq.protocol = protocol;
483 rq.adr = *adr;
484 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
485 if (err)
486 return err;
487 ch->recv = rq.ch->send;
488 ch->peer = rq.ch;
489 rq.ch->recv = ch->send;
490 rq.ch->peer = ch;
491 rq.ch->st = dev->D.st;
492 } else {
493 bp = get_Bprotocol4mask(pmask);
494 if (!bp)
495 return -ENOPROTOOPT;
496 rq2.protocol = protocol;
497 rq2.adr = *adr;
498 rq2.ch = ch;
499 err = bp->create(&rq2);
500 if (err)
501 return err;
502 ch->recv = rq2.ch->send;
503 ch->peer = rq2.ch;
504 rq2.ch->st = dev->D.st;
505 rq.protocol = rq2.protocol;
506 rq.adr = *adr;
507 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
508 if (err) {
509 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
510 return err;
511 }
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;
517 }
518 ch->protocol = protocol;
519 ch->nr = rq.ch->nr;
520 return 0;
521}
522
523int
524create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
525 u_int protocol, struct sockaddr_mISDN *adr)
526{
527 struct channel_req rq;
528 int err;
529
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,
534 adr->tei);
535 rq.protocol = ISDN_P_TE_S0;
536 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
537 rq.protocol = ISDN_P_TE_E1;
538 switch (protocol) {
539 case ISDN_P_LAPD_NT:
540 rq.protocol = ISDN_P_NT_S0;
541 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
542 rq.protocol = ISDN_P_NT_E1;
543 case ISDN_P_LAPD_TE:
544#ifdef PROTOCOL_CHECK
545
546 if (!list_empty(&dev->D.st->layer2)
547 && dev->D.protocol != protocol)
548 return -EBUSY;
549 if (!hlist_empty(&dev->D.st->l1sock.head)
550 && dev->D.protocol != protocol)
551 return -EBUSY;
552#endif
553 ch->recv = mISDN_queue_message;
554 ch->peer = &dev->D.st->own;
555 ch->st = dev->D.st;
556 rq.adr.channel = 0;
557 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
558 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
559 if (err)
560 break;
561 rq.protocol = protocol;
562 rq.adr = *adr;
563 rq.ch = ch;
564 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
565 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
566 if (!err) {
567 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
568 break;
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);
573 }
574 break;
575 default:
576 err = -EPROTONOSUPPORT;
577 }
578 return err;
579}
580
581void
582delete_channel(struct mISDNchannel *ch)
583{
584 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
585 struct mISDNchannel *pch;
586
587 if (!ch->st) {
588 printk(KERN_WARNING "%s: no stack\n", __func__);
589 return;
590 }
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) {
595 if (ch->peer) {
596 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
597 ch->peer = NULL;
598 }
599 return;
600 }
601 switch (ch->protocol) {
602 case ISDN_P_NT_S0:
603 case ISDN_P_TE_S0:
604 case ISDN_P_NT_E1:
605 case ISDN_P_TE_E1:
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);
610 break;
611 case ISDN_P_LAPD_TE:
612 pch = get_channel4id(ch->st, ch->nr);
613 if (pch) {
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);
620 } else
621 printk(KERN_WARNING "%s: no l2 channel\n",
622 __func__);
623 break;
624 case ISDN_P_LAPD_NT:
625 pch = ch->st->dev->teimgr;
626 if (pch) {
627 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
628 } else
629 printk(KERN_WARNING "%s: no l2 channel\n",
630 __func__);
631 break;
632 default:
633 break;
634 }
635 return;
636}
637
638void
639delete_stack(struct mISDNdevice *dev)
640{
641 struct mISDNstack *st = dev->D.st;
642 DECLARE_COMPLETION_ONSTACK(done);
643
644 if (*debug & DEBUG_CORE_FUNC)
645 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
646 st->dev->name);
647 if (dev->teimgr)
648 delete_teimanager(dev->teimgr);
649 if (st->thread) {
650 if (st->notify) {
651 printk(KERN_WARNING "%s: notifier in use\n",
652 __func__);
653 complete(st->notify);
654 }
655 st->notify = &done;
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);
660 }
661 if (!list_empty(&st->layer2))
662 printk(KERN_WARNING "%s: layer2 list not empty\n",
663 __func__);
664 if (!hlist_empty(&st->l1sock.head))
665 printk(KERN_WARNING "%s: layer1 list not empty\n",
666 __func__);
667 kfree(st);
668}
669
670void
671mISDN_initstack(u_int *dp)
672{
673 debug = dp;
674}