Loading...
1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 *
4 * Author Karsten Keil <kkeil@novell.com>
5 *
6 * Copyright 2008 by Karsten Keil <kkeil@novell.com>
7 */
8
9#include <linux/slab.h>
10#include <linux/mISDNif.h>
11#include <linux/kthread.h>
12#include <linux/sched.h>
13#include <linux/sched/cputime.h>
14#include <linux/signal.h>
15
16#include "core.h"
17
18static u_int *debug;
19
20static inline void
21_queue_message(struct mISDNstack *st, struct sk_buff *skb)
22{
23 struct mISDNhead *hh = mISDN_HEAD_P(skb);
24
25 if (*debug & DEBUG_QUEUE_FUNC)
26 printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
27 __func__, hh->prim, hh->id, skb);
28 skb_queue_tail(&st->msgq, skb);
29 if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) {
30 test_and_set_bit(mISDN_STACK_WORK, &st->status);
31 wake_up_interruptible(&st->workq);
32 }
33}
34
35static int
36mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
37{
38 _queue_message(ch->st, skb);
39 return 0;
40}
41
42static struct mISDNchannel *
43get_channel4id(struct mISDNstack *st, u_int id)
44{
45 struct mISDNchannel *ch;
46
47 mutex_lock(&st->lmutex);
48 list_for_each_entry(ch, &st->layer2, list) {
49 if (id == ch->nr)
50 goto unlock;
51 }
52 ch = NULL;
53unlock:
54 mutex_unlock(&st->lmutex);
55 return ch;
56}
57
58static void
59send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
60{
61 struct sock *sk;
62 struct sk_buff *cskb = NULL;
63
64 read_lock(&sl->lock);
65 sk_for_each(sk, &sl->head) {
66 if (sk->sk_state != MISDN_BOUND)
67 continue;
68 if (!cskb)
69 cskb = skb_copy(skb, GFP_ATOMIC);
70 if (!cskb) {
71 printk(KERN_WARNING "%s no skb\n", __func__);
72 break;
73 }
74 if (!sock_queue_rcv_skb(sk, cskb))
75 cskb = NULL;
76 }
77 read_unlock(&sl->lock);
78 dev_kfree_skb(cskb);
79}
80
81static void
82send_layer2(struct mISDNstack *st, struct sk_buff *skb)
83{
84 struct sk_buff *cskb;
85 struct mISDNhead *hh = mISDN_HEAD_P(skb);
86 struct mISDNchannel *ch;
87 int ret;
88
89 if (!st)
90 return;
91 mutex_lock(&st->lmutex);
92 if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) { /* L2 for all */
93 list_for_each_entry(ch, &st->layer2, list) {
94 if (list_is_last(&ch->list, &st->layer2)) {
95 cskb = skb;
96 skb = NULL;
97 } else {
98 cskb = skb_copy(skb, GFP_KERNEL);
99 }
100 if (cskb) {
101 ret = ch->send(ch, cskb);
102 if (ret) {
103 if (*debug & DEBUG_SEND_ERR)
104 printk(KERN_DEBUG
105 "%s ch%d prim(%x) addr(%x)"
106 " err %d\n",
107 __func__, ch->nr,
108 hh->prim, ch->addr, ret);
109 dev_kfree_skb(cskb);
110 }
111 } else {
112 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
113 __func__, ch->nr, ch->addr);
114 goto out;
115 }
116 }
117 } else {
118 list_for_each_entry(ch, &st->layer2, list) {
119 if ((hh->id & MISDN_ID_ADDR_MASK) == ch->addr) {
120 ret = ch->send(ch, skb);
121 if (!ret)
122 skb = NULL;
123 goto out;
124 }
125 }
126 ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb);
127 if (!ret)
128 skb = NULL;
129 else if (*debug & DEBUG_SEND_ERR)
130 printk(KERN_DEBUG
131 "%s mgr prim(%x) err %d\n",
132 __func__, hh->prim, ret);
133 }
134out:
135 mutex_unlock(&st->lmutex);
136 dev_kfree_skb(skb);
137}
138
139static inline int
140send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
141{
142 struct mISDNhead *hh = mISDN_HEAD_P(skb);
143 struct mISDNchannel *ch;
144 int lm;
145
146 lm = hh->prim & MISDN_LAYERMASK;
147 if (*debug & DEBUG_QUEUE_FUNC)
148 printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n",
149 __func__, hh->prim, hh->id, skb);
150 if (lm == 0x1) {
151 if (!hlist_empty(&st->l1sock.head)) {
152 __net_timestamp(skb);
153 send_socklist(&st->l1sock, skb);
154 }
155 return st->layer1->send(st->layer1, skb);
156 } else if (lm == 0x2) {
157 if (!hlist_empty(&st->l1sock.head))
158 send_socklist(&st->l1sock, skb);
159 send_layer2(st, skb);
160 return 0;
161 } else if (lm == 0x4) {
162 ch = get_channel4id(st, hh->id);
163 if (ch)
164 return ch->send(ch, skb);
165 else
166 printk(KERN_WARNING
167 "%s: dev(%s) prim(%x) id(%x) no channel\n",
168 __func__, dev_name(&st->dev->dev), hh->prim,
169 hh->id);
170 } else if (lm == 0x8) {
171 WARN_ON(lm == 0x8);
172 ch = get_channel4id(st, hh->id);
173 if (ch)
174 return ch->send(ch, skb);
175 else
176 printk(KERN_WARNING
177 "%s: dev(%s) prim(%x) id(%x) no channel\n",
178 __func__, dev_name(&st->dev->dev), hh->prim,
179 hh->id);
180 } else {
181 /* broadcast not handled yet */
182 printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
183 __func__, dev_name(&st->dev->dev), hh->prim);
184 }
185 return -ESRCH;
186}
187
188static void
189do_clear_stack(struct mISDNstack *st)
190{
191}
192
193static int
194mISDNStackd(void *data)
195{
196 struct mISDNstack *st = data;
197#ifdef MISDN_MSG_STATS
198 u64 utime, stime;
199#endif
200 int err = 0;
201
202 sigfillset(¤t->blocked);
203 if (*debug & DEBUG_MSG_THREAD)
204 printk(KERN_DEBUG "mISDNStackd %s started\n",
205 dev_name(&st->dev->dev));
206
207 if (st->notify != NULL) {
208 complete(st->notify);
209 st->notify = NULL;
210 }
211
212 for (;;) {
213 struct sk_buff *skb;
214
215 if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
216 test_and_clear_bit(mISDN_STACK_WORK, &st->status);
217 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
218 } else
219 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
220 while (test_bit(mISDN_STACK_WORK, &st->status)) {
221 skb = skb_dequeue(&st->msgq);
222 if (!skb) {
223 test_and_clear_bit(mISDN_STACK_WORK,
224 &st->status);
225 /* test if a race happens */
226 skb = skb_dequeue(&st->msgq);
227 if (!skb)
228 continue;
229 test_and_set_bit(mISDN_STACK_WORK,
230 &st->status);
231 }
232#ifdef MISDN_MSG_STATS
233 st->msg_cnt++;
234#endif
235 err = send_msg_to_layer(st, skb);
236 if (unlikely(err)) {
237 if (*debug & DEBUG_SEND_ERR)
238 printk(KERN_DEBUG
239 "%s: %s prim(%x) id(%x) "
240 "send call(%d)\n",
241 __func__, dev_name(&st->dev->dev),
242 mISDN_HEAD_PRIM(skb),
243 mISDN_HEAD_ID(skb), err);
244 dev_kfree_skb(skb);
245 continue;
246 }
247 if (unlikely(test_bit(mISDN_STACK_STOPPED,
248 &st->status))) {
249 test_and_clear_bit(mISDN_STACK_WORK,
250 &st->status);
251 test_and_clear_bit(mISDN_STACK_RUNNING,
252 &st->status);
253 break;
254 }
255 }
256 if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
257 test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
258 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
259 do_clear_stack(st);
260 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
261 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
262 }
263 if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
264 test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
265 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
266 if (!skb_queue_empty(&st->msgq))
267 test_and_set_bit(mISDN_STACK_WORK,
268 &st->status);
269 }
270 if (test_bit(mISDN_STACK_ABORT, &st->status))
271 break;
272 if (st->notify != NULL) {
273 complete(st->notify);
274 st->notify = NULL;
275 }
276#ifdef MISDN_MSG_STATS
277 st->sleep_cnt++;
278#endif
279 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
280 wait_event_interruptible(st->workq, (st->status &
281 mISDN_STACK_ACTION_MASK));
282 if (*debug & DEBUG_MSG_THREAD)
283 printk(KERN_DEBUG "%s: %s wake status %08lx\n",
284 __func__, dev_name(&st->dev->dev), st->status);
285 test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
286
287 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
288
289 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
290 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
291#ifdef MISDN_MSG_STATS
292 st->stopped_cnt++;
293#endif
294 }
295 }
296#ifdef MISDN_MSG_STATS
297 printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
298 "msg %d sleep %d stopped\n",
299 dev_name(&st->dev->dev), st->msg_cnt, st->sleep_cnt,
300 st->stopped_cnt);
301 task_cputime(st->thread, &utime, &stime);
302 printk(KERN_DEBUG
303 "mISDNStackd daemon for %s utime(%llu) stime(%llu)\n",
304 dev_name(&st->dev->dev), utime, stime);
305 printk(KERN_DEBUG
306 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
307 dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
308 printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
309 dev_name(&st->dev->dev));
310#endif
311 test_and_set_bit(mISDN_STACK_KILLED, &st->status);
312 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
313 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
314 test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
315 skb_queue_purge(&st->msgq);
316 st->thread = NULL;
317 if (st->notify != NULL) {
318 complete(st->notify);
319 st->notify = NULL;
320 }
321 return 0;
322}
323
324static int
325l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
326{
327 if (!ch->st)
328 return -ENODEV;
329 __net_timestamp(skb);
330 _queue_message(ch->st, skb);
331 return 0;
332}
333
334void
335set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
336{
337 ch->addr = sapi | (tei << 8);
338}
339
340void
341__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
342{
343 list_add_tail(&ch->list, &st->layer2);
344}
345
346void
347add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
348{
349 mutex_lock(&st->lmutex);
350 __add_layer2(ch, st);
351 mutex_unlock(&st->lmutex);
352}
353
354static int
355st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
356{
357 if (!ch->st || !ch->st->layer1)
358 return -EINVAL;
359 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
360}
361
362int
363create_stack(struct mISDNdevice *dev)
364{
365 struct mISDNstack *newst;
366 int err;
367 DECLARE_COMPLETION_ONSTACK(done);
368
369 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
370 if (!newst) {
371 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
372 return -ENOMEM;
373 }
374 newst->dev = dev;
375 INIT_LIST_HEAD(&newst->layer2);
376 INIT_HLIST_HEAD(&newst->l1sock.head);
377 rwlock_init(&newst->l1sock.lock);
378 init_waitqueue_head(&newst->workq);
379 skb_queue_head_init(&newst->msgq);
380 mutex_init(&newst->lmutex);
381 dev->D.st = newst;
382 err = create_teimanager(dev);
383 if (err) {
384 printk(KERN_ERR "kmalloc teimanager failed\n");
385 kfree(newst);
386 return err;
387 }
388 dev->teimgr->peer = &newst->own;
389 dev->teimgr->recv = mISDN_queue_message;
390 dev->teimgr->st = newst;
391 newst->layer1 = &dev->D;
392 dev->D.recv = l1_receive;
393 dev->D.peer = &newst->own;
394 newst->own.st = newst;
395 newst->own.ctrl = st_own_ctrl;
396 newst->own.send = mISDN_queue_message;
397 newst->own.recv = mISDN_queue_message;
398 if (*debug & DEBUG_CORE_FUNC)
399 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
400 dev_name(&newst->dev->dev));
401 newst->notify = &done;
402 newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
403 dev_name(&newst->dev->dev));
404 if (IS_ERR(newst->thread)) {
405 err = PTR_ERR(newst->thread);
406 printk(KERN_ERR
407 "mISDN:cannot create kernel thread for %s (%d)\n",
408 dev_name(&newst->dev->dev), err);
409 delete_teimanager(dev->teimgr);
410 kfree(newst);
411 } else
412 wait_for_completion(&done);
413 return err;
414}
415
416int
417connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
418 u_int protocol, struct sockaddr_mISDN *adr)
419{
420 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
421 struct channel_req rq;
422 int err;
423
424
425 if (*debug & DEBUG_CORE_FUNC)
426 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
427 __func__, dev_name(&dev->dev), protocol, adr->dev,
428 adr->channel, adr->sapi, adr->tei);
429 switch (protocol) {
430 case ISDN_P_NT_S0:
431 case ISDN_P_NT_E1:
432 case ISDN_P_TE_S0:
433 case ISDN_P_TE_E1:
434 ch->recv = mISDN_queue_message;
435 ch->peer = &dev->D.st->own;
436 ch->st = dev->D.st;
437 rq.protocol = protocol;
438 rq.adr.channel = adr->channel;
439 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
440 printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
441 dev->id);
442 if (err)
443 return err;
444 write_lock_bh(&dev->D.st->l1sock.lock);
445 sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
446 write_unlock_bh(&dev->D.st->l1sock.lock);
447 break;
448 default:
449 return -ENOPROTOOPT;
450 }
451 return 0;
452}
453
454int
455connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
456 u_int protocol, struct sockaddr_mISDN *adr)
457{
458 struct channel_req rq, rq2;
459 int pmask, err;
460 struct Bprotocol *bp;
461
462 if (*debug & DEBUG_CORE_FUNC)
463 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
464 __func__, dev_name(&dev->dev), protocol,
465 adr->dev, adr->channel, adr->sapi,
466 adr->tei);
467 ch->st = dev->D.st;
468 pmask = 1 << (protocol & ISDN_P_B_MASK);
469 if (pmask & dev->Bprotocols) {
470 rq.protocol = protocol;
471 rq.adr = *adr;
472 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
473 if (err)
474 return err;
475 ch->recv = rq.ch->send;
476 ch->peer = rq.ch;
477 rq.ch->recv = ch->send;
478 rq.ch->peer = ch;
479 rq.ch->st = dev->D.st;
480 } else {
481 bp = get_Bprotocol4mask(pmask);
482 if (!bp)
483 return -ENOPROTOOPT;
484 rq2.protocol = protocol;
485 rq2.adr = *adr;
486 rq2.ch = ch;
487 err = bp->create(&rq2);
488 if (err)
489 return err;
490 ch->recv = rq2.ch->send;
491 ch->peer = rq2.ch;
492 rq2.ch->st = dev->D.st;
493 rq.protocol = rq2.protocol;
494 rq.adr = *adr;
495 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
496 if (err) {
497 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
498 return err;
499 }
500 rq2.ch->recv = rq.ch->send;
501 rq2.ch->peer = rq.ch;
502 rq.ch->recv = rq2.ch->send;
503 rq.ch->peer = rq2.ch;
504 rq.ch->st = dev->D.st;
505 }
506 ch->protocol = protocol;
507 ch->nr = rq.ch->nr;
508 return 0;
509}
510
511int
512create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
513 u_int protocol, struct sockaddr_mISDN *adr)
514{
515 struct channel_req rq;
516 int err;
517
518 if (*debug & DEBUG_CORE_FUNC)
519 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
520 __func__, dev_name(&dev->dev), protocol,
521 adr->dev, adr->channel, adr->sapi,
522 adr->tei);
523 rq.protocol = ISDN_P_TE_S0;
524 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
525 rq.protocol = ISDN_P_TE_E1;
526 switch (protocol) {
527 case ISDN_P_LAPD_NT:
528 rq.protocol = ISDN_P_NT_S0;
529 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
530 rq.protocol = ISDN_P_NT_E1;
531 fallthrough;
532 case ISDN_P_LAPD_TE:
533 ch->recv = mISDN_queue_message;
534 ch->peer = &dev->D.st->own;
535 ch->st = dev->D.st;
536 rq.adr.channel = 0;
537 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
538 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
539 if (err)
540 break;
541 rq.protocol = protocol;
542 rq.adr = *adr;
543 rq.ch = ch;
544 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
545 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
546 if (!err) {
547 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
548 break;
549 add_layer2(rq.ch, dev->D.st);
550 rq.ch->recv = mISDN_queue_message;
551 rq.ch->peer = &dev->D.st->own;
552 rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
553 }
554 break;
555 default:
556 err = -EPROTONOSUPPORT;
557 }
558 return err;
559}
560
561void
562delete_channel(struct mISDNchannel *ch)
563{
564 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
565 struct mISDNchannel *pch;
566
567 if (!ch->st) {
568 printk(KERN_WARNING "%s: no stack\n", __func__);
569 return;
570 }
571 if (*debug & DEBUG_CORE_FUNC)
572 printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
573 dev_name(&ch->st->dev->dev), ch->protocol);
574 if (ch->protocol >= ISDN_P_B_START) {
575 if (ch->peer) {
576 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
577 ch->peer = NULL;
578 }
579 return;
580 }
581 switch (ch->protocol) {
582 case ISDN_P_NT_S0:
583 case ISDN_P_TE_S0:
584 case ISDN_P_NT_E1:
585 case ISDN_P_TE_E1:
586 write_lock_bh(&ch->st->l1sock.lock);
587 sk_del_node_init(&msk->sk);
588 write_unlock_bh(&ch->st->l1sock.lock);
589 ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
590 break;
591 case ISDN_P_LAPD_TE:
592 pch = get_channel4id(ch->st, ch->nr);
593 if (pch) {
594 mutex_lock(&ch->st->lmutex);
595 list_del(&pch->list);
596 mutex_unlock(&ch->st->lmutex);
597 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
598 pch = ch->st->dev->teimgr;
599 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
600 } else
601 printk(KERN_WARNING "%s: no l2 channel\n",
602 __func__);
603 break;
604 case ISDN_P_LAPD_NT:
605 pch = ch->st->dev->teimgr;
606 if (pch) {
607 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
608 } else
609 printk(KERN_WARNING "%s: no l2 channel\n",
610 __func__);
611 break;
612 default:
613 break;
614 }
615 return;
616}
617
618void
619delete_stack(struct mISDNdevice *dev)
620{
621 struct mISDNstack *st = dev->D.st;
622 DECLARE_COMPLETION_ONSTACK(done);
623
624 if (*debug & DEBUG_CORE_FUNC)
625 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
626 dev_name(&st->dev->dev));
627 if (dev->teimgr)
628 delete_teimanager(dev->teimgr);
629 if (st->thread) {
630 if (st->notify) {
631 printk(KERN_WARNING "%s: notifier in use\n",
632 __func__);
633 complete(st->notify);
634 }
635 st->notify = &done;
636 test_and_set_bit(mISDN_STACK_ABORT, &st->status);
637 test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
638 wake_up_interruptible(&st->workq);
639 wait_for_completion(&done);
640 }
641 if (!list_empty(&st->layer2))
642 printk(KERN_WARNING "%s: layer2 list not empty\n",
643 __func__);
644 if (!hlist_empty(&st->l1sock.head))
645 printk(KERN_WARNING "%s: layer1 list not empty\n",
646 __func__);
647 kfree(st);
648}
649
650void
651mISDN_initstack(u_int *dp)
652{
653 debug = dp;
654}
1/*
2 *
3 * Author Karsten Keil <kkeil@novell.com>
4 *
5 * Copyright 2008 by Karsten Keil <kkeil@novell.com>
6 *
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.
10 *
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.
15 *
16 */
17
18#include <linux/slab.h>
19#include <linux/mISDNif.h>
20#include <linux/kthread.h>
21#include <linux/sched.h>
22#include "core.h"
23
24static u_int *debug;
25
26static inline void
27_queue_message(struct mISDNstack *st, struct sk_buff *skb)
28{
29 struct mISDNhead *hh = mISDN_HEAD_P(skb);
30
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);
38 }
39}
40
41static int
42mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
43{
44 _queue_message(ch->st, skb);
45 return 0;
46}
47
48static struct mISDNchannel *
49get_channel4id(struct mISDNstack *st, u_int id)
50{
51 struct mISDNchannel *ch;
52
53 mutex_lock(&st->lmutex);
54 list_for_each_entry(ch, &st->layer2, list) {
55 if (id == ch->nr)
56 goto unlock;
57 }
58 ch = NULL;
59unlock:
60 mutex_unlock(&st->lmutex);
61 return ch;
62}
63
64static void
65send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
66{
67 struct sock *sk;
68 struct sk_buff *cskb = NULL;
69
70 read_lock(&sl->lock);
71 sk_for_each(sk, &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) { /* 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);
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 mgr prim(%x) err %d\n",
139 __func__, hh->prim, 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 /* 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);
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#ifdef MISDN_MSG_STATS
206 cputime_t utime, stime;
207#endif
208 int err = 0;
209
210 sigfillset(¤t->blocked);
211 if (*debug & DEBUG_MSG_THREAD)
212 printk(KERN_DEBUG "mISDNStackd %s started\n",
213 dev_name(&st->dev->dev));
214
215 if (st->notify != NULL) {
216 complete(st->notify);
217 st->notify = NULL;
218 }
219
220 for (;;) {
221 struct sk_buff *skb;
222
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);
226 } else
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);
230 if (!skb) {
231 test_and_clear_bit(mISDN_STACK_WORK,
232 &st->status);
233 /* test if a race happens */
234 skb = skb_dequeue(&st->msgq);
235 if (!skb)
236 continue;
237 test_and_set_bit(mISDN_STACK_WORK,
238 &st->status);
239 }
240#ifdef MISDN_MSG_STATS
241 st->msg_cnt++;
242#endif
243 err = send_msg_to_layer(st, skb);
244 if (unlikely(err)) {
245 if (*debug & DEBUG_SEND_ERR)
246 printk(KERN_DEBUG
247 "%s: %s prim(%x) id(%x) "
248 "send call(%d)\n",
249 __func__, dev_name(&st->dev->dev),
250 mISDN_HEAD_PRIM(skb),
251 mISDN_HEAD_ID(skb), err);
252 dev_kfree_skb(skb);
253 continue;
254 }
255 if (unlikely(test_bit(mISDN_STACK_STOPPED,
256 &st->status))) {
257 test_and_clear_bit(mISDN_STACK_WORK,
258 &st->status);
259 test_and_clear_bit(mISDN_STACK_RUNNING,
260 &st->status);
261 break;
262 }
263 }
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);
267 do_clear_stack(st);
268 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
269 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
270 }
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,
276 &st->status);
277 }
278 if (test_bit(mISDN_STACK_ABORT, &st->status))
279 break;
280 if (st->notify != NULL) {
281 complete(st->notify);
282 st->notify = NULL;
283 }
284#ifdef MISDN_MSG_STATS
285 st->sleep_cnt++;
286#endif
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);
294
295 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
296
297 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
298 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
299#ifdef MISDN_MSG_STATS
300 st->stopped_cnt++;
301#endif
302 }
303 }
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,
308 st->stopped_cnt);
309 task_cputime(st->thread, &utime, &stime);
310 printk(KERN_DEBUG
311 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
312 dev_name(&st->dev->dev), utime, stime);
313 printk(KERN_DEBUG
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));
318#endif
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);
324 st->thread = NULL;
325 if (st->notify != NULL) {
326 complete(st->notify);
327 st->notify = NULL;
328 }
329 return 0;
330}
331
332static int
333l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
334{
335 if (!ch->st)
336 return -ENODEV;
337 __net_timestamp(skb);
338 _queue_message(ch->st, skb);
339 return 0;
340}
341
342void
343set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
344{
345 ch->addr = sapi | (tei << 8);
346}
347
348void
349__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
350{
351 list_add_tail(&ch->list, &st->layer2);
352}
353
354void
355add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
356{
357 mutex_lock(&st->lmutex);
358 __add_layer2(ch, st);
359 mutex_unlock(&st->lmutex);
360}
361
362static int
363st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
364{
365 if (!ch->st || !ch->st->layer1)
366 return -EINVAL;
367 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
368}
369
370int
371create_stack(struct mISDNdevice *dev)
372{
373 struct mISDNstack *newst;
374 int err;
375 DECLARE_COMPLETION_ONSTACK(done);
376
377 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
378 if (!newst) {
379 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
380 return -ENOMEM;
381 }
382 newst->dev = dev;
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);
389 dev->D.st = newst;
390 err = create_teimanager(dev);
391 if (err) {
392 printk(KERN_ERR "kmalloc teimanager failed\n");
393 kfree(newst);
394 return err;
395 }
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);
414 printk(KERN_ERR
415 "mISDN:cannot create kernel thread for %s (%d)\n",
416 dev_name(&newst->dev->dev), err);
417 delete_teimanager(dev->teimgr);
418 kfree(newst);
419 } else
420 wait_for_completion(&done);
421 return err;
422}
423
424int
425connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
426 u_int protocol, struct sockaddr_mISDN *adr)
427{
428 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
429 struct channel_req rq;
430 int err;
431
432
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);
437 switch (protocol) {
438 case ISDN_P_NT_S0:
439 case ISDN_P_NT_E1:
440 case ISDN_P_TE_S0:
441 case ISDN_P_TE_E1:
442 ch->recv = mISDN_queue_message;
443 ch->peer = &dev->D.st->own;
444 ch->st = dev->D.st;
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,
449 dev->id);
450 if (err)
451 return 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);
455 break;
456 default:
457 return -ENOPROTOOPT;
458 }
459 return 0;
460}
461
462int
463connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
464 u_int protocol, struct sockaddr_mISDN *adr)
465{
466 struct channel_req rq, rq2;
467 int pmask, err;
468 struct Bprotocol *bp;
469
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,
474 adr->tei);
475 ch->st = dev->D.st;
476 pmask = 1 << (protocol & ISDN_P_B_MASK);
477 if (pmask & dev->Bprotocols) {
478 rq.protocol = protocol;
479 rq.adr = *adr;
480 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
481 if (err)
482 return err;
483 ch->recv = rq.ch->send;
484 ch->peer = rq.ch;
485 rq.ch->recv = ch->send;
486 rq.ch->peer = ch;
487 rq.ch->st = dev->D.st;
488 } else {
489 bp = get_Bprotocol4mask(pmask);
490 if (!bp)
491 return -ENOPROTOOPT;
492 rq2.protocol = protocol;
493 rq2.adr = *adr;
494 rq2.ch = ch;
495 err = bp->create(&rq2);
496 if (err)
497 return err;
498 ch->recv = rq2.ch->send;
499 ch->peer = rq2.ch;
500 rq2.ch->st = dev->D.st;
501 rq.protocol = rq2.protocol;
502 rq.adr = *adr;
503 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
504 if (err) {
505 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
506 return err;
507 }
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;
513 }
514 ch->protocol = protocol;
515 ch->nr = rq.ch->nr;
516 return 0;
517}
518
519int
520create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
521 u_int protocol, struct sockaddr_mISDN *adr)
522{
523 struct channel_req rq;
524 int err;
525
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,
530 adr->tei);
531 rq.protocol = ISDN_P_TE_S0;
532 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
533 rq.protocol = ISDN_P_TE_E1;
534 switch (protocol) {
535 case ISDN_P_LAPD_NT:
536 rq.protocol = ISDN_P_NT_S0;
537 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
538 rq.protocol = ISDN_P_NT_E1;
539 case ISDN_P_LAPD_TE:
540 ch->recv = mISDN_queue_message;
541 ch->peer = &dev->D.st->own;
542 ch->st = dev->D.st;
543 rq.adr.channel = 0;
544 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
545 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
546 if (err)
547 break;
548 rq.protocol = protocol;
549 rq.adr = *adr;
550 rq.ch = ch;
551 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
552 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
553 if (!err) {
554 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
555 break;
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 */
560 }
561 break;
562 default:
563 err = -EPROTONOSUPPORT;
564 }
565 return err;
566}
567
568void
569delete_channel(struct mISDNchannel *ch)
570{
571 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
572 struct mISDNchannel *pch;
573
574 if (!ch->st) {
575 printk(KERN_WARNING "%s: no stack\n", __func__);
576 return;
577 }
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) {
582 if (ch->peer) {
583 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
584 ch->peer = NULL;
585 }
586 return;
587 }
588 switch (ch->protocol) {
589 case ISDN_P_NT_S0:
590 case ISDN_P_TE_S0:
591 case ISDN_P_NT_E1:
592 case ISDN_P_TE_E1:
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);
597 break;
598 case ISDN_P_LAPD_TE:
599 pch = get_channel4id(ch->st, ch->nr);
600 if (pch) {
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);
607 } else
608 printk(KERN_WARNING "%s: no l2 channel\n",
609 __func__);
610 break;
611 case ISDN_P_LAPD_NT:
612 pch = ch->st->dev->teimgr;
613 if (pch) {
614 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
615 } else
616 printk(KERN_WARNING "%s: no l2 channel\n",
617 __func__);
618 break;
619 default:
620 break;
621 }
622 return;
623}
624
625void
626delete_stack(struct mISDNdevice *dev)
627{
628 struct mISDNstack *st = dev->D.st;
629 DECLARE_COMPLETION_ONSTACK(done);
630
631 if (*debug & DEBUG_CORE_FUNC)
632 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
633 dev_name(&st->dev->dev));
634 if (dev->teimgr)
635 delete_teimanager(dev->teimgr);
636 if (st->thread) {
637 if (st->notify) {
638 printk(KERN_WARNING "%s: notifier in use\n",
639 __func__);
640 complete(st->notify);
641 }
642 st->notify = &done;
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);
647 }
648 if (!list_empty(&st->layer2))
649 printk(KERN_WARNING "%s: layer2 list not empty\n",
650 __func__);
651 if (!hlist_empty(&st->l1sock.head))
652 printk(KERN_WARNING "%s: layer1 list not empty\n",
653 __func__);
654 kfree(st);
655}
656
657void
658mISDN_initstack(u_int *dp)
659{
660 debug = dp;
661}