drm: add DRM_READ/WRITE64 wrappers around readq/writeq.
[linux-2.6/mini2440.git] / drivers / isdn / mISDN / stack.c
blobe2f45019ebf0be504bdcd68538cd47f51109c588
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/mISDNif.h>
19 #include <linux/kthread.h>
20 #include "core.h"
22 static u_int *debug;
24 static inline void
25 _queue_message(struct mISDNstack *st, struct sk_buff *skb)
27 struct mISDNhead *hh = mISDN_HEAD_P(skb);
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);
39 static int
40 mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
42 _queue_message(ch->st, skb);
43 return 0;
46 static struct mISDNchannel *
47 get_channel4id(struct mISDNstack *st, u_int id)
49 struct mISDNchannel *ch;
51 mutex_lock(&st->lmutex);
52 list_for_each_entry(ch, &st->layer2, list) {
53 if (id == ch->nr)
54 goto unlock;
56 ch = NULL;
57 unlock:
58 mutex_unlock(&st->lmutex);
59 return ch;
62 static void
63 send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb)
65 struct hlist_node *node;
66 struct sock *sk;
67 struct sk_buff *cskb = NULL;
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;
79 if (!sock_queue_rcv_skb(sk, cskb))
80 cskb = NULL;
82 read_unlock(&sl->lock);
83 if (cskb)
84 dev_kfree_skb(cskb);
87 static void
88 send_layer2(struct mISDNstack *st, struct sk_buff *skb)
90 struct sk_buff *cskb;
91 struct mISDNhead *hh = mISDN_HEAD_P(skb);
92 struct mISDNchannel *ch;
93 int ret;
95 if (!st)
96 return;
97 mutex_lock(&st->lmutex);
98 if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) { /* L2 for all */
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);
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);
117 } else {
118 printk(KERN_WARNING "%s ch%d addr %x no mem\n",
119 __func__, ch->nr, ch->addr);
120 goto out;
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;
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);
140 out:
141 mutex_unlock(&st->lmutex);
142 if (skb)
143 dev_kfree_skb(skb);
146 static inline int
147 send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb)
149 struct mISDNhead *hh = mISDN_HEAD_P(skb);
150 struct mISDNchannel *ch;
151 int lm;
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);
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__, dev_name(&st->dev->dev), hh->prim,
176 hh->id);
177 } else if (lm == 0x8) {
178 WARN_ON(lm == 0x8);
179 ch = get_channel4id(st, hh->id);
180 if (ch)
181 return ch->send(ch, skb);
182 else
183 printk(KERN_WARNING
184 "%s: dev(%s) prim(%x) id(%x) no channel\n",
185 __func__, dev_name(&st->dev->dev), hh->prim,
186 hh->id);
187 } else {
188 /* broadcast not handled yet */
189 printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n",
190 __func__, dev_name(&st->dev->dev), hh->prim);
192 return -ESRCH;
195 static void
196 do_clear_stack(struct mISDNstack *st)
200 static int
201 mISDNStackd(void *data)
203 struct mISDNstack *st = data;
204 int err = 0;
206 #ifdef CONFIG_SMP
207 lock_kernel();
208 #endif
209 sigfillset(&current->blocked);
210 #ifdef CONFIG_SMP
211 unlock_kernel();
212 #endif
213 if (*debug & DEBUG_MSG_THREAD)
214 printk(KERN_DEBUG "mISDNStackd %s started\n",
215 dev_name(&st->dev->dev));
217 if (st->notify != NULL) {
218 complete(st->notify);
219 st->notify = NULL;
222 for (;;) {
223 struct sk_buff *skb;
225 if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
226 test_and_clear_bit(mISDN_STACK_WORK, &st->status);
227 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
228 } else
229 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
230 while (test_bit(mISDN_STACK_WORK, &st->status)) {
231 skb = skb_dequeue(&st->msgq);
232 if (!skb) {
233 test_and_clear_bit(mISDN_STACK_WORK,
234 &st->status);
235 /* test if a race happens */
236 skb = skb_dequeue(&st->msgq);
237 if (!skb)
238 continue;
239 test_and_set_bit(mISDN_STACK_WORK,
240 &st->status);
242 #ifdef MISDN_MSG_STATS
243 st->msg_cnt++;
244 #endif
245 err = send_msg_to_layer(st, skb);
246 if (unlikely(err)) {
247 if (*debug & DEBUG_SEND_ERR)
248 printk(KERN_DEBUG
249 "%s: %s prim(%x) id(%x) "
250 "send call(%d)\n",
251 __func__, dev_name(&st->dev->dev),
252 mISDN_HEAD_PRIM(skb),
253 mISDN_HEAD_ID(skb), err);
254 dev_kfree_skb(skb);
255 continue;
257 if (unlikely(test_bit(mISDN_STACK_STOPPED,
258 &st->status))) {
259 test_and_clear_bit(mISDN_STACK_WORK,
260 &st->status);
261 test_and_clear_bit(mISDN_STACK_RUNNING,
262 &st->status);
263 break;
266 if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
267 test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
268 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
269 do_clear_stack(st);
270 test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
271 test_and_set_bit(mISDN_STACK_RESTART, &st->status);
273 if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
274 test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
275 test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
276 if (!skb_queue_empty(&st->msgq))
277 test_and_set_bit(mISDN_STACK_WORK,
278 &st->status);
280 if (test_bit(mISDN_STACK_ABORT, &st->status))
281 break;
282 if (st->notify != NULL) {
283 complete(st->notify);
284 st->notify = NULL;
286 #ifdef MISDN_MSG_STATS
287 st->sleep_cnt++;
288 #endif
289 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
290 wait_event_interruptible(st->workq, (st->status &
291 mISDN_STACK_ACTION_MASK));
292 if (*debug & DEBUG_MSG_THREAD)
293 printk(KERN_DEBUG "%s: %s wake status %08lx\n",
294 __func__, dev_name(&st->dev->dev), st->status);
295 test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
297 test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
299 if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
300 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
301 #ifdef MISDN_MSG_STATS
302 st->stopped_cnt++;
303 #endif
306 #ifdef MISDN_MSG_STATS
307 printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
308 "msg %d sleep %d stopped\n",
309 dev_name(&st->dev->dev), st->msg_cnt, st->sleep_cnt,
310 st->stopped_cnt);
311 printk(KERN_DEBUG
312 "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
313 dev_name(&st->dev->dev), st->thread->utime, st->thread->stime);
314 printk(KERN_DEBUG
315 "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
316 dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
317 printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
318 dev_name(&st->dev->dev));
319 #endif
320 test_and_set_bit(mISDN_STACK_KILLED, &st->status);
321 test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
322 test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
323 test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
324 skb_queue_purge(&st->msgq);
325 st->thread = NULL;
326 if (st->notify != NULL) {
327 complete(st->notify);
328 st->notify = NULL;
330 return 0;
333 static int
334 l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
336 if (!ch->st)
337 return -ENODEV;
338 __net_timestamp(skb);
339 _queue_message(ch->st, skb);
340 return 0;
343 void
344 set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
346 ch->addr = sapi | (tei << 8);
349 void
350 __add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
352 list_add_tail(&ch->list, &st->layer2);
355 void
356 add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
358 mutex_lock(&st->lmutex);
359 __add_layer2(ch, st);
360 mutex_unlock(&st->lmutex);
363 static int
364 st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
366 if (!ch->st || ch->st->layer1)
367 return -EINVAL;
368 return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
372 create_stack(struct mISDNdevice *dev)
374 struct mISDNstack *newst;
375 int err;
376 DECLARE_COMPLETION_ONSTACK(done);
378 newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
379 if (!newst) {
380 printk(KERN_ERR "kmalloc mISDN_stack failed\n");
381 return -ENOMEM;
383 newst->dev = dev;
384 INIT_LIST_HEAD(&newst->layer2);
385 INIT_HLIST_HEAD(&newst->l1sock.head);
386 rwlock_init(&newst->l1sock.lock);
387 init_waitqueue_head(&newst->workq);
388 skb_queue_head_init(&newst->msgq);
389 mutex_init(&newst->lmutex);
390 dev->D.st = newst;
391 err = create_teimanager(dev);
392 if (err) {
393 printk(KERN_ERR "kmalloc teimanager failed\n");
394 kfree(newst);
395 return err;
397 dev->teimgr->peer = &newst->own;
398 dev->teimgr->recv = mISDN_queue_message;
399 dev->teimgr->st = newst;
400 newst->layer1 = &dev->D;
401 dev->D.recv = l1_receive;
402 dev->D.peer = &newst->own;
403 newst->own.st = newst;
404 newst->own.ctrl = st_own_ctrl;
405 newst->own.send = mISDN_queue_message;
406 newst->own.recv = mISDN_queue_message;
407 if (*debug & DEBUG_CORE_FUNC)
408 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
409 dev_name(&newst->dev->dev));
410 newst->notify = &done;
411 newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
412 dev_name(&newst->dev->dev));
413 if (IS_ERR(newst->thread)) {
414 err = PTR_ERR(newst->thread);
415 printk(KERN_ERR
416 "mISDN:cannot create kernel thread for %s (%d)\n",
417 dev_name(&newst->dev->dev), err);
418 delete_teimanager(dev->teimgr);
419 kfree(newst);
420 } else
421 wait_for_completion(&done);
422 return err;
426 connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch,
427 u_int protocol, struct sockaddr_mISDN *adr)
429 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
430 struct channel_req rq;
431 int err;
434 if (*debug & DEBUG_CORE_FUNC)
435 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
436 __func__, dev_name(&dev->dev), protocol, adr->dev,
437 adr->channel, adr->sapi, adr->tei);
438 switch (protocol) {
439 case ISDN_P_NT_S0:
440 case ISDN_P_NT_E1:
441 case ISDN_P_TE_S0:
442 case ISDN_P_TE_E1:
443 ch->recv = mISDN_queue_message;
444 ch->peer = &dev->D.st->own;
445 ch->st = dev->D.st;
446 rq.protocol = protocol;
447 rq.adr.channel = adr->channel;
448 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
449 printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
450 dev->id);
451 if (err)
452 return err;
453 write_lock_bh(&dev->D.st->l1sock.lock);
454 sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
455 write_unlock_bh(&dev->D.st->l1sock.lock);
456 break;
457 default:
458 return -ENOPROTOOPT;
460 return 0;
464 connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
465 u_int protocol, struct sockaddr_mISDN *adr)
467 struct channel_req rq, rq2;
468 int pmask, err;
469 struct Bprotocol *bp;
471 if (*debug & DEBUG_CORE_FUNC)
472 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
473 __func__, dev_name(&dev->dev), protocol,
474 adr->dev, adr->channel, adr->sapi,
475 adr->tei);
476 ch->st = dev->D.st;
477 pmask = 1 << (protocol & ISDN_P_B_MASK);
478 if (pmask & dev->Bprotocols) {
479 rq.protocol = protocol;
480 rq.adr = *adr;
481 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
482 if (err)
483 return err;
484 ch->recv = rq.ch->send;
485 ch->peer = rq.ch;
486 rq.ch->recv = ch->send;
487 rq.ch->peer = ch;
488 rq.ch->st = dev->D.st;
489 } else {
490 bp = get_Bprotocol4mask(pmask);
491 if (!bp)
492 return -ENOPROTOOPT;
493 rq2.protocol = protocol;
494 rq2.adr = *adr;
495 rq2.ch = ch;
496 err = bp->create(&rq2);
497 if (err)
498 return err;
499 ch->recv = rq2.ch->send;
500 ch->peer = rq2.ch;
501 rq2.ch->st = dev->D.st;
502 rq.protocol = rq2.protocol;
503 rq.adr = *adr;
504 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
505 if (err) {
506 rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
507 return err;
509 rq2.ch->recv = rq.ch->send;
510 rq2.ch->peer = rq.ch;
511 rq.ch->recv = rq2.ch->send;
512 rq.ch->peer = rq2.ch;
513 rq.ch->st = dev->D.st;
515 ch->protocol = protocol;
516 ch->nr = rq.ch->nr;
517 return 0;
521 create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
522 u_int protocol, struct sockaddr_mISDN *adr)
524 struct channel_req rq;
525 int err;
527 if (*debug & DEBUG_CORE_FUNC)
528 printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
529 __func__, dev_name(&dev->dev), protocol,
530 adr->dev, adr->channel, adr->sapi,
531 adr->tei);
532 rq.protocol = ISDN_P_TE_S0;
533 if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
534 rq.protocol = ISDN_P_TE_E1;
535 switch (protocol) {
536 case ISDN_P_LAPD_NT:
537 rq.protocol = ISDN_P_NT_S0;
538 if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
539 rq.protocol = ISDN_P_NT_E1;
540 case ISDN_P_LAPD_TE:
541 ch->recv = mISDN_queue_message;
542 ch->peer = &dev->D.st->own;
543 ch->st = dev->D.st;
544 rq.adr.channel = 0;
545 err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
546 printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
547 if (err)
548 break;
549 rq.protocol = protocol;
550 rq.adr = *adr;
551 rq.ch = ch;
552 err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
553 printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
554 if (!err) {
555 if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
556 break;
557 add_layer2(rq.ch, dev->D.st);
558 rq.ch->recv = mISDN_queue_message;
559 rq.ch->peer = &dev->D.st->own;
560 rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
562 break;
563 default:
564 err = -EPROTONOSUPPORT;
566 return err;
569 void
570 delete_channel(struct mISDNchannel *ch)
572 struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
573 struct mISDNchannel *pch;
575 if (!ch->st) {
576 printk(KERN_WARNING "%s: no stack\n", __func__);
577 return;
579 if (*debug & DEBUG_CORE_FUNC)
580 printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
581 dev_name(&ch->st->dev->dev), ch->protocol);
582 if (ch->protocol >= ISDN_P_B_START) {
583 if (ch->peer) {
584 ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
585 ch->peer = NULL;
587 return;
589 switch (ch->protocol) {
590 case ISDN_P_NT_S0:
591 case ISDN_P_TE_S0:
592 case ISDN_P_NT_E1:
593 case ISDN_P_TE_E1:
594 write_lock_bh(&ch->st->l1sock.lock);
595 sk_del_node_init(&msk->sk);
596 write_unlock_bh(&ch->st->l1sock.lock);
597 ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
598 break;
599 case ISDN_P_LAPD_TE:
600 pch = get_channel4id(ch->st, ch->nr);
601 if (pch) {
602 mutex_lock(&ch->st->lmutex);
603 list_del(&pch->list);
604 mutex_unlock(&ch->st->lmutex);
605 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
606 pch = ch->st->dev->teimgr;
607 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
608 } else
609 printk(KERN_WARNING "%s: no l2 channel\n",
610 __func__);
611 break;
612 case ISDN_P_LAPD_NT:
613 pch = ch->st->dev->teimgr;
614 if (pch) {
615 pch->ctrl(pch, CLOSE_CHANNEL, NULL);
616 } else
617 printk(KERN_WARNING "%s: no l2 channel\n",
618 __func__);
619 break;
620 default:
621 break;
623 return;
626 void
627 delete_stack(struct mISDNdevice *dev)
629 struct mISDNstack *st = dev->D.st;
630 DECLARE_COMPLETION_ONSTACK(done);
632 if (*debug & DEBUG_CORE_FUNC)
633 printk(KERN_DEBUG "%s: st(%s)\n", __func__,
634 dev_name(&st->dev->dev));
635 if (dev->teimgr)
636 delete_teimanager(dev->teimgr);
637 if (st->thread) {
638 if (st->notify) {
639 printk(KERN_WARNING "%s: notifier in use\n",
640 __func__);
641 complete(st->notify);
643 st->notify = &done;
644 test_and_set_bit(mISDN_STACK_ABORT, &st->status);
645 test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
646 wake_up_interruptible(&st->workq);
647 wait_for_completion(&done);
649 if (!list_empty(&st->layer2))
650 printk(KERN_WARNING "%s: layer2 list not empty\n",
651 __func__);
652 if (!hlist_empty(&st->l1sock.head))
653 printk(KERN_WARNING "%s: layer1 list not empty\n",
654 __func__);
655 kfree(st);
658 void
659 mISDN_initstack(u_int *dp)
661 debug = dp;