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 <linux/smp_lock.h>
21#include "core.h"
22
23static u_int *debug;
24
25static inline void
26_queue_message(struct mISDNstack *st, struct sk_buff *skb)
27{
28 struct mISDNhead *hh = mISDN_HEAD_P(skb);
29
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);
37 }
38}
39
40static int
41mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
42{
43 _queue_message(ch->st, skb);
44 return 0;
45}
46
47static struct mISDNchannel *
48get_channel4id(struct mISDNstack *st, u_int id)
49{
50 struct mISDNchannel *ch;
51
52 mutex_lock(&st->lmutex);
53 list_for_each_entry(ch, &st->layer2, list) {
54 if (id == ch->nr)
55 goto unlock;
56 }
57 ch = NULL;
58unlock:
59 mutex_unlock(&st->lmutex);
60 return ch;
61}
62
63static void
64send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
65{
66 struct hlist_node *node;
67 struct sock *sk;
68 struct sk_buff *cskb = NULL;
69
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;
79 }
80 if (!sock_queue_rcv_skb(sk, cskb))
81 cskb = NULL;
82 }
83 read_unlock(&sl->lock);
84 if (cskb)
85 dev_kfree_skb(cskb);
86}
87
88static void
89send_layer2(struct mISDNstack *st, struct sk_buff *skb)
90{
91 struct sk_buff *cskb;
92 struct mISDNhead *hh = mISDN_HEAD_P(skb);
93 struct mISDNchannel *ch;
94 int ret;
95
96 if (!st)
97 return;
98 mutex_lock(&st->lmutex);
99 if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) {
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);
106 }
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);
117 }
118 } else {
119 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
120 __func__, ch->nr, ch->addr);
121 goto out;
122 }
123 }
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;
131 }
132 }
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);
140 }
141out:
142 mutex_unlock(&st->lmutex);
143 if (skb)
144 dev_kfree_skb(skb);
145}
146
147static inline int
148send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
149{
150 struct mISDNhead *hh = mISDN_HEAD_P(skb);
151 struct mISDNchannel *ch;
152 int lm;
153
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);
162 }
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
190 printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
191 __func__, dev_name(&st->dev->dev), hh->prim);
192 }
193 return -ESRCH;
194}
195
196static void
197do_clear_stack(struct mISDNstack *st)
198{
199}
200
201static int
202mISDNStackd(void *data)
203{
204 struct mISDNstack *st = data;
205 int err = 0;
206
207#ifdef CONFIG_SMP
208 lock_kernel();
209#endif
210 sigfillset(¤t->blocked);
211#ifdef CONFIG_SMP
212 unlock_kernel();
213#endif
214 if (*debug & DEBUG_MSG_THREAD)
215 printk(KERN_DEBUG "mISDNStackd %s started\n",
216 dev_name(&st->dev->dev));
217
218 if (st->notify != NULL) {
219 complete(st->notify);
220 st->notify = NULL;
221 }
222
223 for (;;) {
224 struct sk_buff *skb;
225
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);
229 } else
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);
233 if (!skb) {
234 test_and_clear_bit(mISDN_STACK_WORK,
235 &st->status);
236
237 skb = skb_dequeue(&st->msgq);
238 if (!skb)
239 continue;
240 test_and_set_bit(mISDN_STACK_WORK,
241 &st->status);
242 }
243#ifdef MISDN_MSG_STATS
244 st->msg_cnt++;
245#endif
246 err = send_msg_to_layer(st, skb);
247 if (unlikely(err)) {
248 if (*debug & DEBUG_SEND_ERR)
249 printk(KERN_DEBUG
250 "%s: %s prim(%x) id(%x) "
251 "send call(%d)\n",
252 __func__, dev_name(&st->dev->dev),
253 mISDN_HEAD_PRIM(skb),
254 mISDN_HEAD_ID(skb), err);
255 dev_kfree_skb(skb);
256 continue;
257 }
258 if (unlikely(test_bit(mISDN_STACK_STOPPED,
259 &st->status))) {
260 test_and_clear_bit(mISDN_STACK_WORK,
261 &st->status);
262 test_and_clear_bit(mISDN_STACK_RUNNING,
263 &st->status);
264 break;
265 }
266 }
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);
270 do_clear_stack(st);
271 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
272 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
273 }
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,
279 &st->status);
280 }
281 if (test_bit(mISDN_STACK_ABORT, &st->status))
282 break;
283 if (st->notify != NULL) {
284 complete(st->notify);
285 st->notify = NULL;
286 }
287#ifdef MISDN_MSG_STATS
288 st->sleep_cnt++;
289#endif
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);
297
298 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
299
300 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
301 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
302#ifdef MISDN_MSG_STATS
303 st->stopped_cnt++;
304#endif
305 }
306 }
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,
311 st->stopped_cnt);
312 printk(KERN_DEBUG
313 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
314 dev_name(&st->dev->dev), st->thread->utime, st->thread->stime);
315 printk(KERN_DEBUG
316 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
317 dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
318 printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
319 dev_name(&st->dev->dev));
320#endif
321 test_and_set_bit(mISDN_STACK_KILLED, &st->status);
322 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
323 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
324 test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
325 skb_queue_purge(&st->msgq);
326 st->thread = NULL;
327 if (st->notify != NULL) {
328 complete(st->notify);
329 st->notify = NULL;
330 }
331 return 0;
332}
333
334static int
335l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
336{
337 if (!ch->st)
338 return -ENODEV;
339 __net_timestamp(skb);
340 _queue_message(ch->st, skb);
341 return 0;
342}
343
344void
345set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
346{
347 ch->addr = sapi | (tei << 8);
348}
349
350void
351__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
352{
353 list_add_tail(&ch->list, &st->layer2);
354}
355
356void
357add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
358{
359 mutex_lock(&st->lmutex);
360 __add_layer2(ch, st);
361 mutex_unlock(&st->lmutex);
362}
363
364static int
365st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
366{
367 if (!ch->st || !ch->st->layer1)
368 return -EINVAL;
369 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
370}
371
372int
373create_stack(struct mISDNdevice *dev)
374{
375 struct mISDNstack *newst;
376 int err;
377 DECLARE_COMPLETION_ONSTACK(done);
378
379 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
380 if (!newst) {
381 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
382 return -ENOMEM;
383 }
384 newst->dev = dev;
385 INIT_LIST_HEAD(&newst->layer2);
386 INIT_HLIST_HEAD(&newst->l1sock.head);
387 rwlock_init(&newst->l1sock.lock);
388 init_waitqueue_head(&newst->workq);
389 skb_queue_head_init(&newst->msgq);
390 mutex_init(&newst->lmutex);
391 dev->D.st = newst;
392 err = create_teimanager(dev);
393 if (err) {
394 printk(KERN_ERR "kmalloc teimanager failed\n");
395 kfree(newst);
396 return err;
397 }
398 dev->teimgr->peer = &newst->own;
399 dev->teimgr->recv = mISDN_queue_message;
400 dev->teimgr->st = newst;
401 newst->layer1 = &dev->D;
402 dev->D.recv = l1_receive;
403 dev->D.peer = &newst->own;
404 newst->own.st = newst;
405 newst->own.ctrl = st_own_ctrl;
406 newst->own.send = mISDN_queue_message;
407 newst->own.recv = mISDN_queue_message;
408 if (*debug & DEBUG_CORE_FUNC)
409 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
410 dev_name(&newst->dev->dev));
411 newst->notify = &done;
412 newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
413 dev_name(&newst->dev->dev));
414 if (IS_ERR(newst->thread)) {
415 err = PTR_ERR(newst->thread);
416 printk(KERN_ERR
417 "mISDN:cannot create kernel thread for %s (%d)\n",
418 dev_name(&newst->dev->dev), err);
419 delete_teimanager(dev->teimgr);
420 kfree(newst);
421 } else
422 wait_for_completion(&done);
423 return err;
424}
425
426int
427connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
428 u_int protocol, struct sockaddr_mISDN *adr)
429{
430 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
431 struct channel_req rq;
432 int err;
433
434
435 if (*debug & DEBUG_CORE_FUNC)
436 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
437 __func__, dev_name(&dev->dev), protocol, adr->dev,
438 adr->channel, adr->sapi, adr->tei);
439 switch (protocol) {
440 case ISDN_P_NT_S0:
441 case ISDN_P_NT_E1:
442 case ISDN_P_TE_S0:
443 case ISDN_P_TE_E1:
444 ch->recv = mISDN_queue_message;
445 ch->peer = &dev->D.st->own;
446 ch->st = dev->D.st;
447 rq.protocol = protocol;
448 rq.adr.channel = adr->channel;
449 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
450 printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
451 dev->id);
452 if (err)
453 return err;
454 write_lock_bh(&dev->D.st->l1sock.lock);
455 sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
456 write_unlock_bh(&dev->D.st->l1sock.lock);
457 break;
458 default:
459 return -ENOPROTOOPT;
460 }
461 return 0;
462}
463
464int
465connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
466 u_int protocol, struct sockaddr_mISDN *adr)
467{
468 struct channel_req rq, rq2;
469 int pmask, err;
470 struct Bprotocol *bp;
471
472 if (*debug & DEBUG_CORE_FUNC)
473 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
474 __func__, dev_name(&dev->dev), protocol,
475 adr->dev, adr->channel, adr->sapi,
476 adr->tei);
477 ch->st = dev->D.st;
478 pmask = 1 << (protocol & ISDN_P_B_MASK);
479 if (pmask & dev->Bprotocols) {
480 rq.protocol = protocol;
481 rq.adr = *adr;
482 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
483 if (err)
484 return err;
485 ch->recv = rq.ch->send;
486 ch->peer = rq.ch;
487 rq.ch->recv = ch->send;
488 rq.ch->peer = ch;
489 rq.ch->st = dev->D.st;
490 } else {
491 bp = get_Bprotocol4mask(pmask);
492 if (!bp)
493 return -ENOPROTOOPT;
494 rq2.protocol = protocol;
495 rq2.adr = *adr;
496 rq2.ch = ch;
497 err = bp->create(&rq2);
498 if (err)
499 return err;
500 ch->recv = rq2.ch->send;
501 ch->peer = rq2.ch;
502 rq2.ch->st = dev->D.st;
503 rq.protocol = rq2.protocol;
504 rq.adr = *adr;
505 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
506 if (err) {
507 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
508 return err;
509 }
510 rq2.ch->recv = rq.ch->send;
511 rq2.ch->peer = rq.ch;
512 rq.ch->recv = rq2.ch->send;
513 rq.ch->peer = rq2.ch;
514 rq.ch->st = dev->D.st;
515 }
516 ch->protocol = protocol;
517 ch->nr = rq.ch->nr;
518 return 0;
519}
520
521int
522create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
523 u_int protocol, struct sockaddr_mISDN *adr)
524{
525 struct channel_req rq;
526 int err;
527
528 if (*debug & DEBUG_CORE_FUNC)
529 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
530 __func__, dev_name(&dev->dev), protocol,
531 adr->dev, adr->channel, adr->sapi,
532 adr->tei);
533 rq.protocol = ISDN_P_TE_S0;
534 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
535 rq.protocol = ISDN_P_TE_E1;
536 switch (protocol) {
537 case ISDN_P_LAPD_NT:
538 rq.protocol = ISDN_P_NT_S0;
539 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
540 rq.protocol = ISDN_P_NT_E1;
541 case ISDN_P_LAPD_TE:
542 ch->recv = mISDN_queue_message;
543 ch->peer = &dev->D.st->own;
544 ch->st = dev->D.st;
545 rq.adr.channel = 0;
546 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
547 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
548 if (err)
549 break;
550 rq.protocol = protocol;
551 rq.adr = *adr;
552 rq.ch = ch;
553 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
554 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
555 if (!err) {
556 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
557 break;
558 add_layer2(rq.ch, dev->D.st);
559 rq.ch->recv = mISDN_queue_message;
560 rq.ch->peer = &dev->D.st->own;
561 rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL);
562 }
563 break;
564 default:
565 err = -EPROTONOSUPPORT;
566 }
567 return err;
568}
569
570void
571delete_channel(struct mISDNchannel *ch)
572{
573 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
574 struct mISDNchannel *pch;
575
576 if (!ch->st) {
577 printk(KERN_WARNING "%s: no stack\n", __func__);
578 return;
579 }
580 if (*debug & DEBUG_CORE_FUNC)
581 printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
582 dev_name(&ch->st->dev->dev), ch->protocol);
583 if (ch->protocol >= ISDN_P_B_START) {
584 if (ch->peer) {
585 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
586 ch->peer = NULL;
587 }
588 return;
589 }
590 switch (ch->protocol) {
591 case ISDN_P_NT_S0:
592 case ISDN_P_TE_S0:
593 case ISDN_P_NT_E1:
594 case ISDN_P_TE_E1:
595 write_lock_bh(&ch->st->l1sock.lock);
596 sk_del_node_init(&msk->sk);
597 write_unlock_bh(&ch->st->l1sock.lock);
598 ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
599 break;
600 case ISDN_P_LAPD_TE:
601 pch = get_channel4id(ch->st, ch->nr);
602 if (pch) {
603 mutex_lock(&ch->st->lmutex);
604 list_del(&pch->list);
605 mutex_unlock(&ch->st->lmutex);
606 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
607 pch = ch->st->dev->teimgr;
608 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
609 } else
610 printk(KERN_WARNING "%s: no l2 channel\n",
611 __func__);
612 break;
613 case ISDN_P_LAPD_NT:
614 pch = ch->st->dev->teimgr;
615 if (pch) {
616 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
617 } else
618 printk(KERN_WARNING "%s: no l2 channel\n",
619 __func__);
620 break;
621 default:
622 break;
623 }
624 return;
625}
626
627void
628delete_stack(struct mISDNdevice *dev)
629{
630 struct mISDNstack *st = dev->D.st;
631 DECLARE_COMPLETION_ONSTACK(done);
632
633 if (*debug & DEBUG_CORE_FUNC)
634 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
635 dev_name(&st->dev->dev));
636 if (dev->teimgr)
637 delete_teimanager(dev->teimgr);
638 if (st->thread) {
639 if (st->notify) {
640 printk(KERN_WARNING "%s: notifier in use\n",
641 __func__);
642 complete(st->notify);
643 }
644 st->notify = &done;
645 test_and_set_bit(mISDN_STACK_ABORT, &st->status);
646 test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
647 wake_up_interruptible(&st->workq);
648 wait_for_completion(&done);
649 }
650 if (!list_empty(&st->layer2))
651 printk(KERN_WARNING "%s: layer2 list not empty\n",
652 __func__);
653 if (!hlist_empty(&st->l1sock.head))
654 printk(KERN_WARNING "%s: layer1 list not empty\n",
655 __func__);
656 kfree(st);
657}
658
659void
660mISDN_initstack(u_int *dp)
661{
662 debug = dp;
663}
664
