Linux Kernel  3.7.1
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
stack.c
Go to the documentation of this file.
1 /*
2  *
3  * Author Karsten Keil <[email protected]>
4  *
5  * Copyright 2008 by Karsten Keil <[email protected]>
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 "core.h"
22 
23 static u_int *debug;
24 
25 static 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 
40 static int
41 mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb)
42 {
43  _queue_message(ch->st, skb);
44  return 0;
45 }
46 
47 static struct mISDNchannel *
48 get_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;
58 unlock:
59  mutex_unlock(&st->lmutex);
60  return ch;
61 }
62 
63 static void
64 send_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 
88 static void
89 send_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)
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)
138  "%s mgr prim(%x) err %d\n",
139  __func__, hh->prim, ret);
140  }
141 out:
142  mutex_unlock(&st->lmutex);
143  if (skb)
144  dev_kfree_skb(skb);
145 }
146 
147 static inline int
148 send_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
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
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 
196 static void
197 do_clear_stack(struct mISDNstack *st)
198 {
199 }
200 
201 static int
202 mISDNStackd(void *data)
203 {
204  struct mISDNstack *st = data;
205  int err = 0;
206 
207  sigfillset(&current->blocked);
208  if (*debug & DEBUG_MSG_THREAD)
209  printk(KERN_DEBUG "mISDNStackd %s started\n",
210  dev_name(&st->dev->dev));
211 
212  if (st->notify != NULL) {
213  complete(st->notify);
214  st->notify = NULL;
215  }
216 
217  for (;;) {
218  struct sk_buff *skb;
219 
220  if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
221  test_and_clear_bit(mISDN_STACK_WORK, &st->status);
223  } else
225  while (test_bit(mISDN_STACK_WORK, &st->status)) {
226  skb = skb_dequeue(&st->msgq);
227  if (!skb) {
229  &st->status);
230  /* test if a race happens */
231  skb = skb_dequeue(&st->msgq);
232  if (!skb)
233  continue;
235  &st->status);
236  }
237 #ifdef MISDN_MSG_STATS
238  st->msg_cnt++;
239 #endif
240  err = send_msg_to_layer(st, skb);
241  if (unlikely(err)) {
242  if (*debug & DEBUG_SEND_ERR)
244  "%s: %s prim(%x) id(%x) "
245  "send call(%d)\n",
246  __func__, dev_name(&st->dev->dev),
247  mISDN_HEAD_PRIM(skb),
248  mISDN_HEAD_ID(skb), err);
249  dev_kfree_skb(skb);
250  continue;
251  }
253  &st->status))) {
255  &st->status);
257  &st->status);
258  break;
259  }
260  }
261  if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
264  do_clear_stack(st);
267  }
268  if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
271  if (!skb_queue_empty(&st->msgq))
273  &st->status);
274  }
275  if (test_bit(mISDN_STACK_ABORT, &st->status))
276  break;
277  if (st->notify != NULL) {
278  complete(st->notify);
279  st->notify = NULL;
280  }
281 #ifdef MISDN_MSG_STATS
282  st->sleep_cnt++;
283 #endif
285  wait_event_interruptible(st->workq, (st->status &
287  if (*debug & DEBUG_MSG_THREAD)
288  printk(KERN_DEBUG "%s: %s wake status %08lx\n",
289  __func__, dev_name(&st->dev->dev), st->status);
290  test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
291 
293 
294  if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
296 #ifdef MISDN_MSG_STATS
297  st->stopped_cnt++;
298 #endif
299  }
300  }
301 #ifdef MISDN_MSG_STATS
302  printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d "
303  "msg %d sleep %d stopped\n",
304  dev_name(&st->dev->dev), st->msg_cnt, st->sleep_cnt,
305  st->stopped_cnt);
307  "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n",
308  dev_name(&st->dev->dev), st->thread->utime, st->thread->stime);
310  "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n",
311  dev_name(&st->dev->dev), st->thread->nvcsw, st->thread->nivcsw);
312  printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n",
313  dev_name(&st->dev->dev));
314 #endif
315  test_and_set_bit(mISDN_STACK_KILLED, &st->status);
319  skb_queue_purge(&st->msgq);
320  st->thread = NULL;
321  if (st->notify != NULL) {
322  complete(st->notify);
323  st->notify = NULL;
324  }
325  return 0;
326 }
327 
328 static int
329 l1_receive(struct mISDNchannel *ch, struct sk_buff *skb)
330 {
331  if (!ch->st)
332  return -ENODEV;
333  __net_timestamp(skb);
334  _queue_message(ch->st, skb);
335  return 0;
336 }
337 
338 void
339 set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei)
340 {
341  ch->addr = sapi | (tei << 8);
342 }
343 
344 void
345 __add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
346 {
347  list_add_tail(&ch->list, &st->layer2);
348 }
349 
350 void
351 add_layer2(struct mISDNchannel *ch, struct mISDNstack *st)
352 {
353  mutex_lock(&st->lmutex);
354  __add_layer2(ch, st);
355  mutex_unlock(&st->lmutex);
356 }
357 
358 static int
359 st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
360 {
361  if (!ch->st || !ch->st->layer1)
362  return -EINVAL;
363  return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg);
364 }
365 
366 int
367 create_stack(struct mISDNdevice *dev)
368 {
369  struct mISDNstack *newst;
370  int err;
372 
373  newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL);
374  if (!newst) {
375  printk(KERN_ERR "kmalloc mISDN_stack failed\n");
376  return -ENOMEM;
377  }
378  newst->dev = dev;
379  INIT_LIST_HEAD(&newst->layer2);
380  INIT_HLIST_HEAD(&newst->l1sock.head);
381  rwlock_init(&newst->l1sock.lock);
382  init_waitqueue_head(&newst->workq);
383  skb_queue_head_init(&newst->msgq);
384  mutex_init(&newst->lmutex);
385  dev->D.st = newst;
386  err = create_teimanager(dev);
387  if (err) {
388  printk(KERN_ERR "kmalloc teimanager failed\n");
389  kfree(newst);
390  return err;
391  }
392  dev->teimgr->peer = &newst->own;
393  dev->teimgr->recv = mISDN_queue_message;
394  dev->teimgr->st = newst;
395  newst->layer1 = &dev->D;
396  dev->D.recv = l1_receive;
397  dev->D.peer = &newst->own;
398  newst->own.st = newst;
399  newst->own.ctrl = st_own_ctrl;
400  newst->own.send = mISDN_queue_message;
401  newst->own.recv = mISDN_queue_message;
402  if (*debug & DEBUG_CORE_FUNC)
403  printk(KERN_DEBUG "%s: st(%s)\n", __func__,
404  dev_name(&newst->dev->dev));
405  newst->notify = &done;
406  newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s",
407  dev_name(&newst->dev->dev));
408  if (IS_ERR(newst->thread)) {
409  err = PTR_ERR(newst->thread);
411  "mISDN:cannot create kernel thread for %s (%d)\n",
412  dev_name(&newst->dev->dev), err);
413  delete_teimanager(dev->teimgr);
414  kfree(newst);
415  } else
417  return err;
418 }
419 
420 int
421 connect_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(&dev->dev), protocol, adr->dev,
432  adr->channel, 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  ch->recv = mISDN_queue_message;
439  ch->peer = &dev->D.st->own;
440  ch->st = dev->D.st;
441  rq.protocol = protocol;
442  rq.adr.channel = adr->channel;
443  err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
444  printk(KERN_DEBUG "%s: ret %d (dev %d)\n", __func__, err,
445  dev->id);
446  if (err)
447  return err;
448  write_lock_bh(&dev->D.st->l1sock.lock);
449  sk_add_node(&msk->sk, &dev->D.st->l1sock.head);
450  write_unlock_bh(&dev->D.st->l1sock.lock);
451  break;
452  default:
453  return -ENOPROTOOPT;
454  }
455  return 0;
456 }
457 
458 int
459 connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch,
460  u_int protocol, struct sockaddr_mISDN *adr)
461 {
462  struct channel_req rq, rq2;
463  int pmask, err;
464  struct Bprotocol *bp;
465 
466  if (*debug & DEBUG_CORE_FUNC)
467  printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
468  __func__, dev_name(&dev->dev), protocol,
469  adr->dev, adr->channel, adr->sapi,
470  adr->tei);
471  ch->st = dev->D.st;
472  pmask = 1 << (protocol & ISDN_P_B_MASK);
473  if (pmask & dev->Bprotocols) {
474  rq.protocol = protocol;
475  rq.adr = *adr;
476  err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
477  if (err)
478  return err;
479  ch->recv = rq.ch->send;
480  ch->peer = rq.ch;
481  rq.ch->recv = ch->send;
482  rq.ch->peer = ch;
483  rq.ch->st = dev->D.st;
484  } else {
485  bp = get_Bprotocol4mask(pmask);
486  if (!bp)
487  return -ENOPROTOOPT;
488  rq2.protocol = protocol;
489  rq2.adr = *adr;
490  rq2.ch = ch;
491  err = bp->create(&rq2);
492  if (err)
493  return err;
494  ch->recv = rq2.ch->send;
495  ch->peer = rq2.ch;
496  rq2.ch->st = dev->D.st;
497  rq.protocol = rq2.protocol;
498  rq.adr = *adr;
499  err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
500  if (err) {
501  rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL);
502  return err;
503  }
504  rq2.ch->recv = rq.ch->send;
505  rq2.ch->peer = rq.ch;
506  rq.ch->recv = rq2.ch->send;
507  rq.ch->peer = rq2.ch;
508  rq.ch->st = dev->D.st;
509  }
510  ch->protocol = protocol;
511  ch->nr = rq.ch->nr;
512  return 0;
513 }
514 
515 int
516 create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch,
517  u_int protocol, struct sockaddr_mISDN *adr)
518 {
519  struct channel_req rq;
520  int err;
521 
522  if (*debug & DEBUG_CORE_FUNC)
523  printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
524  __func__, dev_name(&dev->dev), protocol,
525  adr->dev, adr->channel, adr->sapi,
526  adr->tei);
527  rq.protocol = ISDN_P_TE_S0;
528  if (dev->Dprotocols & (1 << ISDN_P_TE_E1))
529  rq.protocol = ISDN_P_TE_E1;
530  switch (protocol) {
531  case ISDN_P_LAPD_NT:
532  rq.protocol = ISDN_P_NT_S0;
533  if (dev->Dprotocols & (1 << ISDN_P_NT_E1))
534  rq.protocol = ISDN_P_NT_E1;
535  case ISDN_P_LAPD_TE:
536  ch->recv = mISDN_queue_message;
537  ch->peer = &dev->D.st->own;
538  ch->st = dev->D.st;
539  rq.adr.channel = 0;
540  err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq);
541  printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err);
542  if (err)
543  break;
544  rq.protocol = protocol;
545  rq.adr = *adr;
546  rq.ch = ch;
547  err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq);
548  printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err);
549  if (!err) {
550  if ((protocol == ISDN_P_LAPD_NT) && !rq.ch)
551  break;
552  add_layer2(rq.ch, dev->D.st);
553  rq.ch->recv = mISDN_queue_message;
554  rq.ch->peer = &dev->D.st->own;
555  rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */
556  }
557  break;
558  default:
559  err = -EPROTONOSUPPORT;
560  }
561  return err;
562 }
563 
564 void
565 delete_channel(struct mISDNchannel *ch)
566 {
567  struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch);
568  struct mISDNchannel *pch;
569 
570  if (!ch->st) {
571  printk(KERN_WARNING "%s: no stack\n", __func__);
572  return;
573  }
574  if (*debug & DEBUG_CORE_FUNC)
575  printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__,
576  dev_name(&ch->st->dev->dev), ch->protocol);
577  if (ch->protocol >= ISDN_P_B_START) {
578  if (ch->peer) {
579  ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL);
580  ch->peer = NULL;
581  }
582  return;
583  }
584  switch (ch->protocol) {
585  case ISDN_P_NT_S0:
586  case ISDN_P_TE_S0:
587  case ISDN_P_NT_E1:
588  case ISDN_P_TE_E1:
589  write_lock_bh(&ch->st->l1sock.lock);
590  sk_del_node_init(&msk->sk);
591  write_unlock_bh(&ch->st->l1sock.lock);
592  ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL);
593  break;
594  case ISDN_P_LAPD_TE:
595  pch = get_channel4id(ch->st, ch->nr);
596  if (pch) {
597  mutex_lock(&ch->st->lmutex);
598  list_del(&pch->list);
599  mutex_unlock(&ch->st->lmutex);
600  pch->ctrl(pch, CLOSE_CHANNEL, NULL);
601  pch = ch->st->dev->teimgr;
602  pch->ctrl(pch, CLOSE_CHANNEL, NULL);
603  } else
604  printk(KERN_WARNING "%s: no l2 channel\n",
605  __func__);
606  break;
607  case ISDN_P_LAPD_NT:
608  pch = ch->st->dev->teimgr;
609  if (pch) {
610  pch->ctrl(pch, CLOSE_CHANNEL, NULL);
611  } else
612  printk(KERN_WARNING "%s: no l2 channel\n",
613  __func__);
614  break;
615  default:
616  break;
617  }
618  return;
619 }
620 
621 void
622 delete_stack(struct mISDNdevice *dev)
623 {
624  struct mISDNstack *st = dev->D.st;
626 
627  if (*debug & DEBUG_CORE_FUNC)
628  printk(KERN_DEBUG "%s: st(%s)\n", __func__,
629  dev_name(&st->dev->dev));
630  if (dev->teimgr)
631  delete_teimanager(dev->teimgr);
632  if (st->thread) {
633  if (st->notify) {
634  printk(KERN_WARNING "%s: notifier in use\n",
635  __func__);
636  complete(st->notify);
637  }
638  st->notify = &done;
639  test_and_set_bit(mISDN_STACK_ABORT, &st->status);
640  test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
641  wake_up_interruptible(&st->workq);
643  }
644  if (!list_empty(&st->layer2))
645  printk(KERN_WARNING "%s: layer2 list not empty\n",
646  __func__);
647  if (!hlist_empty(&st->l1sock.head))
648  printk(KERN_WARNING "%s: layer1 list not empty\n",
649  __func__);
650  kfree(st);
651 }
652 
653 void
655 {
656  debug = dp;
657 }