retronetworking
/
mISDN
13
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
mISDN/drivers/isdn/hardware/mISDN/stack.c

1338 lines
33 KiB
C
Raw Blame History

/* $Id$
*
* Author Karsten Keil (keil@isdn4linux.de)
*
* This file is released under the GPLv2
*
*/
#include "core.h"
static LIST_HEAD(mISDN_stacklist);
static rwlock_t stacklist_lock = RW_LOCK_UNLOCKED;
static LIST_HEAD(mISDN_instlist);
static rwlock_t instlist_lock = RW_LOCK_UNLOCKED;
int
get_stack_cnt(void)
{
int cnt = 0;
mISDNstack_t *st;
read_lock(&stacklist_lock);
list_for_each_entry(st, &mISDN_stacklist, list)
cnt++;
read_unlock(&stacklist_lock);
return(cnt);
}
void
get_stack_info(struct sk_buff *skb)
{
mISDN_head_t *hp;
mISDNstack_t *cst, *st;
stack_info_t *si;
int i;
hp = mISDN_HEAD_P(skb);
if (hp->addr == 0) {
hp->dinfo = get_stack_cnt();
hp->len = 0;
return;
} else if (hp->addr <= 127 && hp->addr <= get_stack_cnt()) {
/* stack nr */
i = 1;
read_lock(&stacklist_lock);
list_for_each_entry(st, &mISDN_stacklist, list) {
if (i == hp->addr)
break;
i++;
}
read_unlock(&stacklist_lock);
} else
st = get_stack4id(hp->addr);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: addr(%08x) st(%p) id(%08x)\n", __FUNCTION__, hp->addr, st,
st ? st->id : 0);
if (!st) {
hp->len = -ENODEV;
return;
} else {
si = (stack_info_t *)skb->data;
memset(si, 0, sizeof(stack_info_t));
si->id = st->id;
si->extentions = st->extentions;
if (st->mgr)
si->mgr = st->mgr->id;
else
si->mgr = 0;
memcpy(&si->pid, &st->pid, sizeof(mISDN_pid_t));
memcpy(&si->para, &st->para, sizeof(mISDN_stPara_t));
if (st->clone)
si->clone = st->clone->id;
else
si->clone = 0;
if (st->master)
si->master = st->master->id;
else
si->master = 0;
si->instcnt = 0;
for (i = 0; i <= MAX_LAYER_NR; i++) {
if (st->i_array[i]) {
si->inst[si->instcnt] = st->i_array[i]->id;
si->instcnt++;
}
}
si->childcnt = 0;
list_for_each_entry(cst, &st->childlist, list) {
si->child[si->childcnt] = cst->id;
si->childcnt++;
}
hp->len = sizeof(stack_info_t);
if (si->childcnt>2)
hp->len += (si->childcnt-2)*sizeof(int);
}
skb_put(skb, hp->len);
}
static int
get_free_stackid(mISDNstack_t *mst, int flag)
{
u_int id = 0, found;
mISDNstack_t *st;
if (!mst) {
while(id < STACK_ID_MAX) {
found = 0;
id += STACK_ID_INC;
read_lock(&stacklist_lock);
list_for_each_entry(st, &mISDN_stacklist, list) {
if (st->id == id) {
found++;
break;
}
}
read_unlock(&stacklist_lock);
if (!found)
return(id);
}
} else if (flag & FLG_CLONE_STACK) {
id = mst->id | FLG_CLONE_STACK;
while(id < CLONE_ID_MAX) {
found = 0;
id += CLONE_ID_INC;
st = mst->clone;
while (st) {
if (st->id == id) {
found++;
break;
}
st = st->clone;
}
if (!found)
return(id);
}
} else if (flag & FLG_CHILD_STACK) {
id = mst->id | FLG_CHILD_STACK;
while(id < CHILD_ID_MAX) {
id += CHILD_ID_INC;
found = 0;
list_for_each_entry(st, &mst->childlist, list) {
if (st->id == id) {
found++;
break;
}
}
if (!found)
return(id);
}
}
return(0);
}
mISDNstack_t *
get_stack4id(u_int id)
{
mISDNstack_t *cst, *st;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "get_stack4id(%x)\n", id);
if (!id) /* 0 isn't a valid id */
return(NULL);
read_lock(&stacklist_lock);
list_for_each_entry(st, &mISDN_stacklist, list) {
if (id == st->id) {
read_unlock(&stacklist_lock);
return(st);
}
if ((id & FLG_CHILD_STACK) && ((id & MASTER_ID_MASK) == st->id)) {
list_for_each_entry(cst, &st->childlist, list) {
if (cst->id == id) {
read_unlock(&stacklist_lock);
return(cst);
}
}
}
if ((id & FLG_CLONE_STACK) && ((id & MASTER_ID_MASK) == st->id)) {
cst = st->clone;
while (cst) {
if (cst->id == id) {
read_unlock(&stacklist_lock);
return(cst);
}
cst = cst->clone;
}
}
}
read_unlock(&stacklist_lock);
return(NULL);
}
mISDNinstance_t *
getlayer4lay(mISDNstack_t *st, int layermask)
{
int i;
if (!st) {
int_error();
return(NULL);
}
for (i = 0; i <= MAX_LAYER_NR; i++) {
if (st->i_array[i] && (st->i_array[i]->pid.layermask & layermask))
return(st->i_array[i]);
}
return(NULL);
}
static mISDNinstance_t *
get_nextlayer(mISDNstack_t *st, u_int addr)
{
mISDNinstance_t *inst=NULL;
int layer = addr & LAYER_ID_MASK;
if (!(addr & FLG_MSG_TARGET)) {
switch(addr & MSG_DIR_MASK) {
case FLG_MSG_DOWN:
if (addr & FLG_MSG_CLONED) {
/* OK */
} else
layer -= LAYER_ID_INC;
break;
case FLG_MSG_UP:
if (addr & FLG_MSG_CLONED) {
/* OK */
} else
layer += LAYER_ID_INC;
break;
case MSG_TO_OWNER:
break;
default: /* broadcast */
int_errtxt("st(%08x) addr(%08x) wrong address", st->id, addr);
return(NULL);
}
}
if ((layer < 0) || (layer > MAX_LAYER_NR)) {
int_errtxt("st(%08x) addr(%08x) layer %d out of range", st->id, addr, layer);
return(NULL);
}
inst = st->i_array[layer];
if (core_debug & DEBUG_QUEUE_FUNC)
printk(KERN_DEBUG "%s: st(%08x) addr(%08x) -> inst(%08x)\n",
__FUNCTION__, st->id, addr, inst ? inst->id : 0);
return(inst);
}
mISDNinstance_t *
get_instance(mISDNstack_t *st, int layer_nr, int protocol)
{
mISDNinstance_t *inst=NULL;
int i;
if (!st) {
int_error();
return(NULL);
}
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "get_instance st(%08x) lnr(%d) prot(%x)\n",
st->id, layer_nr, protocol);
if ((layer_nr < 0) || (layer_nr > MAX_LAYER_NR)) {
int_errtxt("lnr %d", layer_nr);
return(NULL);
}
list_for_each_entry(inst, &st->prereg, list) {
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "get_instance prereg(%p, %x) lm %x/%x prot %x/%x\n",
inst, inst->id, inst->pid.layermask, ISDN_LAYER(layer_nr),
inst->pid.protocol[layer_nr], protocol);
if ((inst->pid.layermask & ISDN_LAYER(layer_nr)) &&
(inst->pid.protocol[layer_nr] == protocol)) {
i = register_layer(st, inst);
if (i) {
int_errtxt("error(%d) register preregistered inst(%08x) on st(%08x)", i, inst->id, st->id);
return(NULL);
}
return(inst);
}
}
for (i = 0; i <= MAX_LAYER_NR; i++) {
if ((inst = st->i_array[i])) {
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "get_instance inst%d(%p, %x) lm %x/%x prot %x/%x\n",
i,inst, inst->id, inst->pid.layermask, ISDN_LAYER(layer_nr),
inst->pid.protocol[layer_nr], protocol);
if ((inst->pid.layermask & ISDN_LAYER(layer_nr)) &&
(inst->pid.protocol[layer_nr] == protocol))
return(inst);
}
}
return(NULL);
}
mISDNinstance_t *
get_instance4id(u_int id)
{
mISDNinstance_t *inst;
read_lock(&instlist_lock);
list_for_each_entry(inst, &mISDN_instlist, list)
if (inst->id == id) {
read_unlock(&instlist_lock);
return(inst);
}
read_unlock(&instlist_lock);
return(NULL);
}
#ifdef OBSOLETE
int
get_layermask(mISDNlayer_t *layer)
{
int mask = 0;
if (layer->inst)
mask |= layer->inst->pid.layermask;
return(mask);
}
int
insertlayer(mISDNstack_t *st, mISDNlayer_t *layer, int layermask)
{
mISDNlayer_t *item;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s(%p, %p, %x)\n",
__FUNCTION__, st, layer, layermask);
if (!st || !layer) {
int_error();
return(-EINVAL);
}
if (list_empty(&st->layerlist)) {
list_add(&layer->list, &st->layerlist);
} else {
list_for_each_entry(item, &st->layerlist, list) {
if (layermask < get_layermask(item)) {
list_add_tail(&layer->list, &item->list);
return(0);
}
}
list_add_tail(&layer->list, &st->layerlist);
}
return(0);
}
#endif
inline void
_queue_message(mISDNstack_t *st, struct sk_buff *skb)
{
skb_queue_tail(&st->msgq, skb);
if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) {
test_and_set_bit(mISDN_STACK_WORK, &st->status);
wake_up_interruptible(&st->workq);
}
}
int
mISDN_queue_message(mISDNinstance_t *inst, u_int aflag, struct sk_buff *skb)
{
mISDN_head_t *hh = mISDN_HEAD_P(skb);
mISDNstack_t *st = inst->st;
u_int id;
if (core_debug & DEBUG_QUEUE_FUNC)
printk(KERN_DEBUG "%s(%08x, %x, prim(%x))\n", __FUNCTION__,
inst->id, aflag, hh->prim);
if (aflag & FLG_MSG_TARGET) {
id = aflag;
} else {
id = (inst->id & INST_ID_MASK) | aflag;
}
if ((aflag & MSG_DIR_MASK) == FLG_MSG_DOWN) {
if (inst->parent) {
inst = inst->parent;
st = inst->st;
id = (inst->id & INST_ID_MASK) | FLG_MSG_TARGET | FLG_MSG_CLONED | FLG_MSG_DOWN;
}
}
if (!st)
return(-EINVAL);
if (st->id == 0 || test_bit(mISDN_STACK_ABORT, &st->status))
return(-EBUSY);
if (inst->id == 0) { /* instance is not initialised */
if (!(aflag & FLG_MSG_TARGET)) {
id &= INST_ID_MASK;
id |= (st->id & INST_ID_MASK) | aflag | FLG_INSTANCE;
}
}
if (test_bit(mISDN_STACK_KILLED, &st->status))
return(-EBUSY);
if ((st->id & STACK_ID_MASK) != (id & STACK_ID_MASK)) {
int_errtxt("stack id not match st(%08x) id(%08x) inst(%08x) aflag(%08x) prim(%x)",
st->id, id, inst->id, aflag, hh->prim);
}
hh->addr = id;
_queue_message(st, skb);
return(0);
}
static void
do_broadcast(mISDNstack_t *st, struct sk_buff *skb)
{
mISDN_head_t *hh = mISDN_HEAD_P(skb);
mISDNinstance_t *inst = NULL;
struct sk_buff *c_skb = NULL;
int i, err;
for(i=0; i<=MAX_LAYER_NR; i++) {
if (i == (hh->addr & LAYER_ID_MASK))
continue; // skip own layer
inst = st->i_array[i];
if (!inst)
continue; // maybe we have a gap
if (!c_skb)
c_skb = skb_copy(skb, GFP_KERNEL); // we need a new private copy
if (!c_skb)
break; // stop here when copy not possible
if (core_debug & DEBUG_MSG_THREAD_INFO)
printk(KERN_DEBUG "%s: inst(%08x) msg call addr(%08x) prim(%x)\n",
__FUNCTION__, inst->id, hh->addr, hh->prim);
if (inst->function) {
err = inst->function(inst, c_skb);
if (!err)
c_skb = NULL; /* function consumed the skb */
if (core_debug & DEBUG_MSG_THREAD_INFO)
printk(KERN_DEBUG "%s: inst(%08x) msg call return %d\n",
__FUNCTION__, inst->id, err);
} else {
if (core_debug & DEBUG_MSG_THREAD_ERR)
printk(KERN_DEBUG "%s: instance(%08x) no function\n",
__FUNCTION__, inst->id);
}
}
if (c_skb)
dev_kfree_skb(c_skb);
dev_kfree_skb(skb);
}
static void
release_layers(mISDNstack_t *st, u_int prim)
{
int i;
for (i = 0; i <= MAX_LAYER_NR; i++) {
if (st->i_array[i]) {
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%p) inst%d(%p):%x %s lm(%x)\n",
__FUNCTION__, st, i, st->i_array[i], st->i_array[i]->id,
st->i_array[i]->name, st->i_array[i]->pid.layermask);
st->i_array[i]->obj->own_ctrl(st->i_array[i], prim, NULL);
}
}
}
static void
do_clear_stack(mISDNstack_t *st) {
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%08x)\n", __FUNCTION__, st->id);
kfree(st->pid.pbuf);
memset(&st->pid, 0, sizeof(mISDN_pid_t));
memset(&st->para, 0, sizeof(mISDN_stPara_t));
release_layers(st, MGR_UNREGLAYER | REQUEST);
}
static int
mISDNStackd(void *data)
{
mISDNstack_t *st = data;
int err = 0;
#ifdef CONFIG_SMP
lock_kernel();
#endif
MAKEDAEMON("mISDNStackd");
sigfillset(&current->blocked);
st->thread = current;
#ifdef CONFIG_SMP
unlock_kernel();
#endif
if ( core_debug & DEBUG_THREADS)
printk(KERN_DEBUG "mISDNStackd started for id(%08x)\n", st->id);
for (;;) {
struct sk_buff *skb, *c_skb;
mISDN_head_t *hh;
if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
test_and_clear_bit(mISDN_STACK_WORK, &st->status);
test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
} else
test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
while (test_bit(mISDN_STACK_WORK, &st->status)) {
mISDNinstance_t *inst;
skb = skb_dequeue(&st->msgq);
if (!skb) {
test_and_clear_bit(mISDN_STACK_WORK, &st->status);
/* test if a race happens */
if (!(skb = skb_dequeue(&st->msgq)))
continue;
test_and_set_bit(mISDN_STACK_WORK, &st->status);
}
#ifdef MISDN_MSG_STATS
st->msg_cnt++;
#endif
hh = mISDN_HEAD_P(skb);
if (hh->prim == (MGR_CLEARSTACK | REQUEST)) {
mISDN_headext_t *hhe = (mISDN_headext_t *)hh;
if (test_and_set_bit(mISDN_STACK_CLEARING, &st->status)) {
int_errtxt("double clearing");
}
if (hhe->data[0]) {
if (st->notify) {
int_errtxt("notify already set");
up(st->notify);
}
st->notify = hhe->data[0];
}
dev_kfree_skb(skb);
continue;
}
if ((hh->addr & MSG_DIR_MASK) == MSG_BROADCAST) {
do_broadcast(st, skb);
continue;
}
inst = get_nextlayer(st, hh->addr);
if (!inst) {
if (core_debug & DEBUG_MSG_THREAD_ERR)
printk(KERN_DEBUG "%s: st(%08x) no instance for addr(%08x) prim(%x) dinfo(%x)\n",
__FUNCTION__, st->id, hh->addr, hh->prim, hh->dinfo);
dev_kfree_skb(skb);
continue;
}
if (inst->clone && ((hh->addr & MSG_DIR_MASK) == FLG_MSG_UP)) {
u_int id = (inst->clone->id & INST_ID_MASK) | FLG_MSG_TARGET | FLG_MSG_CLONED | FLG_MSG_UP;
#ifdef MISDN_MSG_STATS
st->clone_cnt++;
#endif
c_skb = skb_copy(skb, GFP_KERNEL);
if (c_skb) {
if (core_debug & DEBUG_MSG_THREAD_INFO)
printk(KERN_DEBUG "%s: inst(%08x) msg clone msg to(%08x) caddr(%08x) prim(%x)\n",
__FUNCTION__, inst->id, inst->clone->id, id, hh->prim);
err = mISDN_queue_message(inst->clone, id, c_skb);
if (err) {
if (core_debug & DEBUG_MSG_THREAD_ERR)
printk(KERN_DEBUG "%s: clone instance(%08x) cannot queue msg(%08x) err(%d)\n",
__FUNCTION__, inst->clone->id, id, err);
dev_kfree_skb(c_skb);
}
} else {
printk(KERN_WARNING "%s OOM on msg cloning inst(%08x) caddr(%08x) prim(%x) len(%d)\n",
__FUNCTION__, inst->id, id, hh->prim, skb->len);
}
}
if (core_debug & DEBUG_MSG_THREAD_INFO)
printk(KERN_DEBUG "%s: inst(%08x) msg call addr(%08x) prim(%x)\n",
__FUNCTION__, inst->id, hh->addr, hh->prim);
if (!inst->function) {
if (core_debug & DEBUG_MSG_THREAD_ERR)
printk(KERN_DEBUG "%s: instance(%08x) no function\n",
__FUNCTION__, inst->id);
dev_kfree_skb(skb);
continue;
}
err = inst->function(inst, skb);
if (err) {
if (core_debug & DEBUG_MSG_THREAD_ERR)
printk(KERN_DEBUG "%s: instance(%08x)->function return(%d)\n",
__FUNCTION__, inst->id, err);
dev_kfree_skb(skb);
continue;
}
if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) {
test_and_clear_bit(mISDN_STACK_WORK, &st->status);
test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
break;
}
}
if (test_bit(mISDN_STACK_CLEARING, &st->status)) {
test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
do_clear_stack(st);
test_and_clear_bit(mISDN_STACK_CLEARING, &st->status);
test_and_set_bit(mISDN_STACK_RESTART, &st->status);
}
if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) {
test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
test_and_set_bit(mISDN_STACK_RUNNING, &st->status);
if (!skb_queue_empty(&st->msgq))
test_and_set_bit(mISDN_STACK_WORK, &st->status);
}
if (test_bit(mISDN_STACK_ABORT, &st->status))
break;
if (st->notify != NULL) {
up(st->notify);
st->notify = NULL;
}
#ifdef MISDN_MSG_STATS
st->sleep_cnt++;
#endif
test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
wait_event_interruptible(st->workq, (st->status & mISDN_STACK_ACTION_MASK));
if (core_debug & DEBUG_MSG_THREAD_INFO)
printk(KERN_DEBUG "%s: %08x wake status %08lx\n", __FUNCTION__, st->id, st->status);
test_and_set_bit(mISDN_STACK_ACTIVE, &st->status);
test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status);
if (test_bit(mISDN_STACK_STOPPED, &st->status)) {
test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
#ifdef MISDN_MSG_STATS
st->stopped_cnt++;
#endif
}
}
#ifdef MISDN_MSG_STATS
printk(KERN_DEBUG "mISDNStackd daemon for id(%08x) proceed %d msg %d clone %d sleep %d stopped\n",
st->id, st->msg_cnt, st->clone_cnt, st->sleep_cnt, st->stopped_cnt);
printk(KERN_DEBUG "mISDNStackd daemon for id(%08x) utime(%ld) stime(%ld)\n", st->id, st->thread->utime, st->thread->stime);
printk(KERN_DEBUG "mISDNStackd daemon for id(%08x) nvcsw(%ld) nivcsw(%ld)\n", st->id, st->thread->nvcsw, st->thread->nivcsw);
printk(KERN_DEBUG "mISDNStackd daemon for id(%08x) killed now\n", st->id);
#endif
test_and_set_bit(mISDN_STACK_KILLED, &st->status);
test_and_clear_bit(mISDN_STACK_RUNNING, &st->status);
test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status);
test_and_clear_bit(mISDN_STACK_ABORT, &st->status);
discard_queue(&st->msgq);
st->thread = NULL;
if (st->notify != NULL) {
up(st->notify);
st->notify = NULL;
}
return(0);
}
int
mISDN_start_stack_thread(mISDNstack_t *st)
{
int err = 0;
if (st->thread == NULL && test_bit(mISDN_STACK_KILLED, &st->status)) {
test_and_clear_bit(mISDN_STACK_KILLED, &st->status);
kernel_thread(mISDNStackd, (void *)st, 0);
} else
err = -EBUSY;
return(err);
}
mISDNstack_t *
new_stack(mISDNstack_t *master, mISDNinstance_t *inst)
{
mISDNstack_t *newst;
int err;
u_long flags;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "create %s stack inst(%p)\n",
master ? "child" : "master", inst);
if (!(newst = kmalloc(sizeof(mISDNstack_t), GFP_ATOMIC))) {
printk(KERN_ERR "kmalloc mISDN_stack failed\n");
return(NULL);
}
memset(newst, 0, sizeof(mISDNstack_t));
INIT_LIST_HEAD(&newst->list);
INIT_LIST_HEAD(&newst->childlist);
INIT_LIST_HEAD(&newst->prereg);
init_waitqueue_head(&newst->workq);
skb_queue_head_init(&newst->msgq);
if (!master) {
if (inst && inst->st) {
master = inst->st;
while(master->clone)
master = master->clone;
newst->id = get_free_stackid(inst->st, FLG_CLONE_STACK);
newst->master = master;
master->clone = newst;
master = NULL;
} else {
newst->id = get_free_stackid(NULL, 0);
}
} else {
newst->id = get_free_stackid(master, FLG_CHILD_STACK);
}
newst->mgr = inst;
if (master) {
list_add_tail(&newst->list, &master->childlist);
newst->parent = master;
} else if (!(newst->id & FLG_CLONE_STACK)) {
write_lock_irqsave(&stacklist_lock, flags);
list_add_tail(&newst->list, &mISDN_stacklist);
write_unlock_irqrestore(&stacklist_lock, flags);
}
if (inst) {
inst->st = newst;
}
err = mISDN_register_sysfs_stack(newst);
if (err) {
// FIXME error handling
printk(KERN_ERR "Stack id %x not registered in sysfs\n", newst->id);
}
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "Stack id %x added\n", newst->id);
kernel_thread(mISDNStackd, (void *)newst, 0);
return(newst);
}
int
mISDN_start_stop(mISDNstack_t *st, int start)
{
int ret;
if (start) {
ret = test_and_clear_bit(mISDN_STACK_STOPPED, &st->status);
test_and_set_bit(mISDN_STACK_WAKEUP, &st->status);
if (!skb_queue_empty(&st->msgq))
test_and_set_bit(mISDN_STACK_WORK, &st->status);
wake_up_interruptible(&st->workq);
} else
ret = test_and_set_bit(mISDN_STACK_STOPPED, &st->status);
return(ret);
}
int
do_for_all_layers(void *data, u_int prim, void *arg)
{
mISDNstack_t *st = data;
int i;
if (!st) {
int_error();
return(-EINVAL);
}
for (i = 0; i <= MAX_LAYER_NR; i++) {
if (st->i_array[i]) {
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%p) inst%d(%p):%x %s prim(%x) arg(%p)\n",
__FUNCTION__, st, i, st->i_array[i], st->i_array[i]->id,
st->i_array[i]->name, prim, arg);
st->i_array[i]->obj->own_ctrl(st->i_array[i], prim, arg);
}
}
return(0);
}
int
change_stack_para(mISDNstack_t *st, u_int prim, mISDN_stPara_t *stpara)
{
int changed = 0;
if (!st) {
int_error();
return(-EINVAL);
}
if (prim == (MGR_ADDSTPARA | REQUEST)) {
if (!stpara) {
int_error();
return(-EINVAL);
}
prim = MGR_ADDSTPARA | INDICATION;
if (stpara->maxdatalen > 0 && stpara->maxdatalen < st->para.maxdatalen) {
changed++;
st->para.maxdatalen = stpara->maxdatalen;
}
if (stpara->up_headerlen > st->para.up_headerlen) {
changed++;
st->para.up_headerlen = stpara->up_headerlen;
}
if (stpara->down_headerlen > st->para.down_headerlen) {
changed++;
st->para.down_headerlen = stpara->down_headerlen;
}
if (!changed)
return(0);
stpara = &st->para;
} else if (prim == (MGR_CLRSTPARA | REQUEST)) {
prim = MGR_CLRSTPARA | INDICATION;
memset(&st->para, 0, sizeof(mISDN_stPara_t));
stpara = NULL;
}
return(do_for_all_layers(st, prim, stpara));
}
static int
delete_stack(mISDNstack_t *st)
{
DECLARE_MUTEX_LOCKED(sem);
u_long flags;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%p:%08x)\n", __FUNCTION__, st, st->id);
mISDN_unregister_sysfs_st(st);
if (st->parent)
st->parent = NULL;
if (!list_empty(&st->prereg)) {
mISDNinstance_t *inst, *ni;
int_errtxt("st(%08x)->prereg not empty\n", st->id);
list_for_each_entry_safe(inst, ni, &st->prereg, list) {
int_errtxt("inst(%p:%08x) preregistered", inst, inst->id);
list_del(&inst->list);
}
}
if (st->thread) {
if (st->thread != current) {
if (st->notify) {
int_error();
up(st->notify);
}
st->notify = &sem;
}
test_and_set_bit(mISDN_STACK_ABORT, &st->status);
mISDN_start_stop(st, 1);
if (st->thread != current) /* we cannot wait for us */
down(&sem);
}
release_layers(st, MGR_RELEASE | INDICATION);
write_lock_irqsave(&stacklist_lock, flags);
list_del(&st->list);
write_unlock_irqrestore(&stacklist_lock, flags);
kfree(st);
return(0);
}
int
release_stack(mISDNstack_t *st) {
int err;
mISDNstack_t *cst, *nst;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%p)\n", __FUNCTION__, st);
list_for_each_entry_safe(cst, nst, &st->childlist, list) {
if ((err = delete_stack(cst))) {
return(err);
}
}
if (st->clone) {
st->clone->master = st->master;
}
if (st->master) {
st->master->clone = st->clone;
} else if (st->clone) { /* no master left -> delete clone too */
delete_stack(st->clone);
st->clone = NULL;
}
if ((err = delete_stack(st)))
return(err);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: mISDN_stacklist(%p<-%p->%p)\n", __FUNCTION__,
mISDN_stacklist.prev, &mISDN_stacklist, mISDN_stacklist.next);
return(0);
}
void
cleanup_object(mISDNobject_t *obj)
{
mISDNstack_t *st, *nst;
mISDNinstance_t *inst;
int i;
read_lock(&stacklist_lock);
list_for_each_entry_safe(st, nst, &mISDN_stacklist, list) {
for (i = 0; i < MAX_LAYER_NR; i++) {
inst = st->i_array[i];
if (inst && inst->obj == obj) {
read_unlock(&stacklist_lock);
inst->obj->own_ctrl(st, MGR_RELEASE | INDICATION, inst);
read_lock(&stacklist_lock);
}
}
}
read_unlock(&stacklist_lock);
}
void
check_stacklist(void)
{
mISDNstack_t *st, *nst;
read_lock(&stacklist_lock);
if (!list_empty(&mISDN_stacklist)) {
printk(KERN_WARNING "mISDNcore mISDN_stacklist not empty\n");
list_for_each_entry_safe(st, nst, &mISDN_stacklist, list) {
printk(KERN_WARNING "mISDNcore st %x still in list\n", st->id);
if (list_empty(&st->list)) {
printk(KERN_WARNING "mISDNcore st == next\n");
break;
}
}
}
read_unlock(&stacklist_lock);
}
void
release_stacks(mISDNobject_t *obj)
{
mISDNstack_t *st, *tmp;
int rel, i;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: obj(%p) %s\n", __FUNCTION__, obj, obj->name);
read_lock(&stacklist_lock);
list_for_each_entry_safe(st, tmp, &mISDN_stacklist, list) {
rel = 0;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%p)\n", __FUNCTION__, st);
for (i = 0; i <= MAX_LAYER_NR; i++) {
if (!st->i_array[i])
continue;
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: inst%d(%p)\n", __FUNCTION__, i, st->i_array[i]);
if (st->i_array[i]->obj == obj)
rel++;
}
if (rel) {
read_unlock(&stacklist_lock);
release_stack(st);
read_lock(&stacklist_lock);
}
}
read_unlock(&stacklist_lock);
if (obj->refcnt)
printk(KERN_WARNING "release_stacks obj %s refcnt is %d\n",
obj->name, obj->refcnt);
}
#ifdef OBSOLETE
static void
get_free_instid(mISDNstack_t *st, mISDNinstance_t *inst) {
mISDNinstance_t *il;
inst->id = mISDN_get_lowlayer(inst->pid.layermask)<<20;
inst->id |= FLG_INSTANCE;
if (st) {
inst->id |= st->id;
} else {
list_for_each_entry(il, &mISDN_instlist, list) {
if (il->id == inst->id) {
if ((inst->id & IF_INSTMASK) >= INST_ID_MAX) {
inst->id = 0;
return;
}
inst->id += LAYER_ID_INC;
il = list_entry(mISDN_instlist.next, mISDNinstance_t, list);
}
}
}
}
#endif
int
register_layer(mISDNstack_t *st, mISDNinstance_t *inst)
{
int idx, err;
mISDNinstance_t *dup;
u_long flags;
if (!inst || !st)
return(-EINVAL);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s:st(%p) inst(%p/%p) lmask(%x) id(%x)\n",
__FUNCTION__, st, inst, inst->obj,
inst->pid.layermask, inst->id);
if (inst->id) { /* already registered */
// if (inst->st || !st) {
int_errtxt("register duplicate %08x %p %p",
inst->id, inst->st, st);
return(-EBUSY);
//}
}
/*
* To simplify registration we assume that our stacks are
* always build with monoton increasing layernumbers from
* bottom (HW,L0) to highest number
*/
// if (st) {
for (idx = 0; idx <= MAX_LAYER_NR; idx++)
if (!st->i_array[idx])
break;
if (idx > MAX_LAYER_NR) {
int_errtxt("stack %08x overflow", st->id);
return(-EXFULL);
}
inst->regcnt++;
st->i_array[idx] = inst;
inst->id = st->id | FLG_INSTANCE | idx;
dup = get_instance4id(inst->id);
if (dup) {
int_errtxt("register duplicate %08x i1(%p) i2(%p) i1->st(%p) i2->st(%p) st(%p)",
inst->id, inst, dup, inst->st, dup->st, st);
inst->regcnt--;
st->i_array[idx] = NULL;
inst->id = 0;
return(-EBUSY);
}
// }
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: inst(%p/%p) id(%x)\n", __FUNCTION__,
inst, inst->obj, inst->id);
if (!list_empty(&inst->list)) {
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: register preregistered instance st(%p/%p)\n",
__FUNCTION__, st, inst->st);
list_del_init(&inst->list);
}
inst->st = st;
write_lock_irqsave(&instlist_lock, flags);
list_add_tail(&inst->list, &mISDN_instlist);
write_unlock_irqrestore(&instlist_lock, flags);
err = mISDN_register_sysfs_inst(inst);
if (err) {
// FIXME error handling
printk(KERN_ERR "%s: register_sysfs failed %d st(%08x) inst(%08x)\n",
__FUNCTION__, err, st->id, inst->id);
}
return(0);
}
int
preregister_layer(mISDNstack_t *st, mISDNinstance_t *inst)
{
if (!inst || !st)
return(-EINVAL);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s:st(%08x) inst(%p:%08x) lmask(%x)\n",
__FUNCTION__, st->id, inst, inst->id, inst->pid.layermask);
if (inst->id) {
/* already registered */
int_errtxt("register duplicate %08x %p %p",
inst->id, inst->st, st);
return(-EBUSY);
}
inst->st = st;
list_add_tail(&inst->list, &st->prereg);
return(0);
}
int
unregister_instance(mISDNinstance_t *inst) {
int i;
u_long flags;
if (!inst)
return(-EINVAL);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%p) inst(%p):%x lay(%x)\n",
__FUNCTION__, inst->st, inst, inst->id, inst->pid.layermask);
mISDN_unregister_sysfs_inst(inst);
if (inst->st && inst->id) {
i = inst->id & LAYER_ID_MASK;
if (i > MAX_LAYER_NR) {
int_errtxt("unregister %08x st(%08x) wrong layer", inst->id, inst->st->id);
return(-EINVAL);
}
if (inst->st->i_array[i] == inst) {
inst->regcnt--;
inst->st->i_array[i] = NULL;
} else if (inst->st->i_array[i]) {
int_errtxt("unregister %08x st(%08x) wrong instance %08x",
inst->id, inst->st->id, inst->st->i_array[i]->id);
return(-EINVAL);
} else
printk(KERN_WARNING "unregister %08x st(%08x) not in stack",
inst->id, inst->st->id);
if (inst->st && (inst->st->mgr != inst))
inst->st = NULL;
}
if (inst->parent) { /*we are cloned */
inst->parent->clone = inst->clone;
if (inst->clone)
inst->clone->parent = inst->parent;
inst->clone = NULL;
inst->parent = NULL;
} else if (inst->clone) {
/* deleting the top level master of a clone */
/* FIXME: should be handled somehow, maybe unregister the clone */
int_errtxt("removed master(%08x) of clone(%08x)", inst->id, inst->clone->id);
inst->clone->parent = NULL;
inst->clone = NULL;
}
write_lock_irqsave(&instlist_lock, flags);
if (inst->list.prev && inst->list.next)
list_del_init(&inst->list);
else
int_errtxt("uninitialized list inst(%08x)", inst->id);
inst->id = 0;
write_unlock_irqrestore(&instlist_lock, flags);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: mISDN_instlist(%p<-%p->%p)\n", __FUNCTION__,
mISDN_instlist.prev, &mISDN_instlist, mISDN_instlist.next);
return(0);
}
int
copy_pid(mISDN_pid_t *dpid, mISDN_pid_t *spid, u_char *pbuf)
{
memcpy(dpid, spid, sizeof(mISDN_pid_t));
if (spid->pbuf && spid->maxplen) {
if (!pbuf) {
int_error();
return(-ENOMEM);
}
dpid->pbuf = pbuf;
memcpy(dpid->pbuf, spid->pbuf, spid->maxplen);
}
return(0);
}
int
set_stack(mISDNstack_t *st, mISDN_pid_t *pid)
{
int err, i;
u_char *pbuf = NULL;
mISDNinstance_t *inst;
// mISDNlayer_t *hl, *hln;
if (!st || !pid) {
int_error();
return(-EINVAL);
}
if (!st->mgr || !st->mgr->obj) {
int_error();
return(-EINVAL);
}
if (pid->pbuf)
pbuf = kmalloc(pid->maxplen, GFP_ATOMIC);
err = copy_pid(&st->pid, pid, pbuf);
if (err)
return(err);
memcpy(&st->mgr->pid, &st->pid, sizeof(mISDN_pid_t));
if (!mISDN_SetHandledPID(st->mgr->obj, &st->mgr->pid)) {
int_error();
return(-ENOPROTOOPT);
} else {
mISDN_RemoveUsedPID(pid, &st->mgr->pid);
}
err = mISDN_ctrl(st, MGR_REGLAYER | REQUEST, st->mgr);
if (err) {
int_error();
return(err);
}
while (pid->layermask) {
inst = get_next_instance(st, pid);
if (!inst) {
int_error();
mISDN_ctrl(st, MGR_CLEARSTACK| REQUEST, (void *)1);
return(-ENOPROTOOPT);
}
mISDN_RemoveUsedPID(pid, &inst->pid);
}
if (!list_empty(&st->prereg))
int_errtxt("st(%08x)->prereg not empty\n", st->id);
for (i = 0; i <= MAX_LAYER_NR; i++) {
inst = st->i_array[i];
if (!inst)
break;
if (!inst->obj) {
int_error();
continue;
}
if (!inst->obj->own_ctrl) {
int_error();
continue;
}
inst->obj->own_ctrl(inst, MGR_SETSTACK | INDICATION, NULL);
}
return(0);
}
int
clear_stack(mISDNstack_t *st, int wait) {
struct sk_buff *skb;
mISDN_headext_t *hhe;
if (!st)
return(-EINVAL);
if (core_debug & DEBUG_CORE_FUNC)
printk(KERN_DEBUG "%s: st(%08x)\n", __FUNCTION__, st->id);
if (!(skb = alloc_skb(8, GFP_ATOMIC)))
return(-ENOMEM);
hhe = mISDN_HEADEXT_P(skb);
hhe->prim = MGR_CLEARSTACK | REQUEST;
hhe->addr = st->id;
if (wait) {
DECLARE_MUTEX_LOCKED(sem);
hhe->data[0] = &sem;
_queue_message(st, skb);
if (st->thread == current) {/* we cannot wait for us */
int_error();
return(-EBUSY);
}
down(&sem);
} else {
hhe->data[0] = NULL;
_queue_message(st, skb);
}
return(0);
}
static int
test_stack_protocol(mISDNstack_t *st, u_int l1prot, u_int l2prot, u_int l3prot)
{
int cnt = MAX_LAYER_NR + 1, ret = 1;
mISDN_pid_t pid;
mISDNinstance_t *inst;
clear_stack(st,1);
memset(&pid, 0, sizeof(mISDN_pid_t));
pid.layermask = ISDN_LAYER(1);
if (!(((l2prot == 2) || (l2prot == 0x40)) && (l3prot == 1)))
pid.layermask |= ISDN_LAYER(2);
if (!(l3prot == 1))
pid.layermask |= ISDN_LAYER(3);
pid.protocol[1] = l1prot | ISDN_PID_LAYER(1) | ISDN_PID_BCHANNEL_BIT;
if (pid.layermask & ISDN_LAYER(2))
pid.protocol[2] = l2prot | ISDN_PID_LAYER(2) | ISDN_PID_BCHANNEL_BIT;
if (pid.layermask & ISDN_LAYER(3))
pid.protocol[3] = l3prot | ISDN_PID_LAYER(3) | ISDN_PID_BCHANNEL_BIT;
copy_pid(&st->pid, &pid, NULL);
memcpy(&st->mgr->pid, &pid, sizeof(mISDN_pid_t));
if (!mISDN_SetHandledPID(st->mgr->obj, &st->mgr->pid)) {
memset(&st->pid, 0, sizeof(mISDN_pid_t));
return(-ENOPROTOOPT);
} else {
mISDN_RemoveUsedPID(&pid, &st->mgr->pid);
}
if (!pid.layermask) {
memset(&st->pid, 0, sizeof(mISDN_pid_t));
return(0);
}
ret = mISDN_ctrl(st, MGR_REGLAYER | REQUEST, st->mgr);
if (ret) {
clear_stack(st, 1);
return(ret);
}
while (pid.layermask && cnt--) {
inst = get_next_instance(st, &pid);
if (!inst) {
ret = -ENOPROTOOPT;
break;
}
mISDN_RemoveUsedPID(&pid, &inst->pid);
}
if (!cnt)
ret = -ENOPROTOOPT;
clear_stack(st, 1);
return(ret);
}
static u_int validL1pid4L2[ISDN_PID_IDX_MAX + 1] = {
0x022d,
0x03ff,
0x0000,
0x0000,
0x0010,
0x022d,
0x03ff,
0x0380,
0x022d,
0x022d,
0x022d,
0x01c6,
0x0000,
};
static u_int validL2pid4L3[ISDN_PID_IDX_MAX + 1] = {
0x1fff,
0x0000,
0x0101,
0x0101,
0x0010,
0x0010,
0x0000,
0x00c0,
0x0000,
};
int
evaluate_stack_pids(mISDNstack_t *st, mISDN_pid_t *pid)
{
int err;
mISDN_pid_t pidmask;
u_int l1bitm, l2bitm, l3bitm;
u_int l1idx, l2idx, l3idx;
if (!st || !pid) {
int_error();
return(-EINVAL);
}
if (!st->mgr || !st->mgr->obj) {
int_error();
return(-EINVAL);
}
copy_pid(&pidmask, pid, NULL);
memset(pid, 0, sizeof(mISDN_pid_t));
for (l1idx=0; l1idx <= ISDN_PID_IDX_MAX; l1idx++) {
l1bitm = 1 << l1idx;
if (!(pidmask.protocol[1] & l1bitm))
continue;
for (l2idx=0; l2idx <= ISDN_PID_IDX_MAX; l2idx++) {
l2bitm = 1 << l2idx;
if (!(pidmask.protocol[2] & l2bitm))
continue;
if (!(validL1pid4L2[l2idx] & l1bitm))
continue;
for (l3idx=0; l3idx <= ISDN_PID_IDX_MAX; l3idx++) {
err = 1;
l3bitm = 1 << l3idx;
if (!(pidmask.protocol[3] & l3bitm))
continue;
if (!(validL2pid4L3[l3idx] & l2bitm))
continue;
err = test_stack_protocol(st, l1bitm, l2bitm, l3bitm);
if (!err) {
pid->protocol[3] |= l3bitm;
pid->protocol[2] |= l2bitm;
pid->protocol[1] |= l1bitm;
}
}
}
}
clear_stack(st, 1);
return(0);
}
EXPORT_SYMBOL(mISDN_queue_message);
Reference in New Issue View Git Blame Copy Permalink