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