base/ipc/netrpc/netrpc.c

Go to the documentation of this file.

/*
 * Copyright (C) 1999-2009 Paolo Mantegazza <mantegazza@aero.polimi.it>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */


#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/version.h>
#include <linux/timer.h>
#include <linux/unistd.h>
#include <asm/uaccess.h>

#include <net/ip.h>

#include <rtai_schedcore.h>
#include <rtai_prinher.h>
#include <rtai_netrpc.h>
#include <rtai_sem.h>
#include <rtai_mbx.h>

MODULE_LICENSE("GPL");

#define COMPILE_ANYHOW  // RTNet is not available but we want to compile anyhow
#include <rtdm.h>
#include "rtnetP.h"

/* ethernet support(s) we want to use: 1 -> DO, 0 -> DO NOT */

#define SOFT_RTNET  1

#ifdef CONFIG_RTAI_NETRPC_RTNET
#define HARD_RTNET  1
#else
#define HARD_RTNET  0
#endif

/* end of ethernet support(s) we want to use */

#if HARD_RTNET
#ifndef COMPILE_ANYHOW
#include <rtnet.h>  // must be the true RTNet header file
#endif
#define MSG_SOFT  0
#define MSG_HARD  1
#define hard_rt_socket              rt_dev_socket
#define hard_rt_bind                rt_dev_bind
#define hard_rt_close               rt_dev_close
#define hard_rt_socket_callback     hard_rt_socket_callback
#define hard_rt_recvfrom            rt_dev_recvfrom
#define hard_rt_sendto              rt_dev_sendto
#else
#define MSG_SOFT 0
#define MSG_HARD 0
#define hard_rt_socket(a, b, c)     portslot[i].socket[0]
#define hard_rt_bind(a, b, c)
#define hard_rt_close(a)
#define hard_rt_socket_callback     soft_rt_socket_callback
#define hard_rt_recvfrom            soft_rt_recvfrom
#define hard_rt_sendto              soft_rt_sendto
#endif

#define LOCALHOST  "127.0.0.1"

#define BASEPORT  (NETRPC_BASEPORT)

#define NETRPC_STACK_SIZE  6000

static unsigned long MaxStubs = MAX_STUBS;
RTAI_MODULE_PARM(MaxStubs, ulong);
static int MaxStubsMone;

static unsigned long MaxSocks = MAX_SOCKS;
RTAI_MODULE_PARM(MaxSocks, ulong);

static int StackSize = NETRPC_STACK_SIZE;
RTAI_MODULE_PARM(StackSize, int);

static char *ThisNode = LOCALHOST;
RTAI_MODULE_PARM(ThisNode, charp);

static char *ThisSoftNode = 0;
RTAI_MODULE_PARM(ThisSoftNode, charp);

static char *ThisHardNode = 0;
RTAI_MODULE_PARM(ThisHardNode, charp);

#define MAX_DFUN_EXT  16
static struct rt_fun_entry *rt_net_rpc_fun_ext[MAX_DFUN_EXT];

static unsigned long this_node[2];

#define PRTSRVNAME  0xFFFFFFFF
struct portslot_t { struct portslot_t *p; long task; int indx, place, socket[2], hard; unsigned long long owner; SEM sem; void *msg; struct sockaddr_in addr; MBX *mbx; unsigned long name;  RTIME timeout; int recovered; };
static spinlock_t portslot_lock = SPIN_LOCK_UNLOCKED;
static volatile int portslotsp;

static spinlock_t stub_lock = SPIN_LOCK_UNLOCKED;
static volatile int stubssp = 1;


static struct portslot_t *portslot;
static struct sockaddr_in SPRT_ADDR;
struct recovery_msg { int hard, priority; unsigned long long owner; struct sockaddr addr; };
static struct { int in, out; struct recovery_msg *msg; } recovery;
static spinlock_t recovery_lock = SPIN_LOCK_UNLOCKED;

#if HARD_RTNET
int hard_rt_socket_callback(int fd, void *func, void *arg)
{
    struct rtnet_callback args = { func, arg };
    return(rt_dev_ioctl(fd, RTNET_RTIOC_CALLBACK, &args));
}
#endif

static inline struct portslot_t *get_portslot(void)
{
    unsigned long flags;

    flags = rt_spin_lock_irqsave(&portslot_lock);
    if (portslotsp < MaxSocks) {
        struct portslot_t *p;
        p = portslot[portslotsp++].p;
        rt_spin_unlock_irqrestore(flags, &portslot_lock);
        return p;
    }
    rt_spin_unlock_irqrestore(flags, &portslot_lock);
    return 0;
}

static inline int gvb_portslot(struct portslot_t *portslotp)
{
    unsigned long flags;

    flags = rt_spin_lock_irqsave(&portslot_lock);
    if (portslotsp > MaxStubs) {
        struct portslot_t *tmp_p;
        int tmp_place;
        tmp_p = portslot[--portslotsp].p;
        tmp_place = portslotp->place;
        portslotp->place = portslotsp;
        portslot[portslotsp].p = portslotp;
        tmp_p->place = tmp_place;
        portslot[tmp_place].p = tmp_p;
        rt_spin_unlock_irqrestore(flags, &portslot_lock);
        return 0;
    }
    rt_spin_unlock_irqrestore(flags, &portslot_lock);
    return -EINVAL;
}

static inline void check_portslot(unsigned long node, int port, struct portslot_t **p)
{
//  unsigned long flags;
    int i;
    struct portslot_t *p_old;
    
    p_old = *p; 
//  flags = rt_spin_lock_irqsave(&portslot_lock);
    for (i = MaxStubs; i < portslotsp; i++) {
        if (portslot[i].p->addr.sin_port == htons(port) && portslot[i].p->addr.sin_addr.s_addr == node) {
            *p = portslot[i].p;
            break;
        }   
    }
//  rt_spin_unlock_irqrestore(flags, &portslot_lock);
    if (p_old != *p)    {
        gvb_portslot(p_old);
    }
    
}

#define NETRPC_TIMER_FREQ 50
static struct timer_list timer;
static SEM timer_sem;

static void timer_fun(unsigned long none)
{
    if (timer_sem.count < 0) {
        rt_sem_signal(&timer_sem);
        timer.expires = jiffies + HZ/NETRPC_TIMER_FREQ;
        add_timer(&timer);
    }
}

static int (*encode)(struct portslot_t *portslotp, void *msg, int size, int where);
static int (*decode)(struct portslot_t *portslotp, void *msg, int size, int where);

void set_netrpc_encoding(void *encode_fun, void *decode_fun, void *ext)
{
    encode = encode_fun;
    decode = decode_fun;
    rt_net_rpc_fun_ext[1] = ext;
}

struct req_rel_msg { long long op, port, priority, hard; unsigned long long owner, name, rem_node, chkspare;};

struct par_t { long long mach, priority, base_priority, argsize, rsize, fun_ext_timed, type; unsigned long long owner, partypes; long a[1]; };

struct reply_t { long long wsize, w2size, myport; unsigned long long retval; char msg[1], msg1[1]; };

static inline int argconv(void *ain, void *aout, int send_mach, int argsize, unsigned int partypes)
{
#define recv_mach  (sizeof(long))
    int argsizeout;
    if (send_mach == recv_mach) {
        memcpy(aout, ain, argsize);
        return argsize;
    }
    argsizeout = 0;
    if (send_mach == 4 && recv_mach == 8) {
        long *out = aout;
        int *in = ain;
        while (argsize) {
            argsize -= 4;
            argsizeout += 8;
            switch(partypes & WDWMSK) {
                case SINT: 
                    *out++ = *in++;
                    break;
                case UINT:
                    *out++ = (unsigned int)*in++;
                    break;
                case VADR:
                    *out++ = reset_kadr(*in++);
                    break;
                case RTIM: 
                    *out++ = (long)*in++;
                    in++;
                    argsize -= 4;
                    break;
            }
            partypes >>= WDW;
        }
    } else {
        unsigned long *out = aout;
        unsigned long long *in = ain;
        while (argsize) {
            argsize -= 8;
            argsizeout += 4;
            switch(partypes & WDWMSK) {
                case SINT: 
                case UINT:
                case VADR:
                    *out++ = *in++;
                    break;
                case RTIM: 
                    *((unsigned long long *)out++) = *in++;
                    out++;
                    argsizeout += 4;
                    break;
            }
            partypes >>= WDW;
        }
    }
    return argsizeout;
#undef recv_mach
}

static void net_resume_task(int sock, struct portslot_t *p)
{
    int all_ok;
    RT_TASK *my;
    my = _rt_whoami();
    all_ok = 1;
    if ((p->indx > 0) && (p->indx < MaxStubs)) {
        if (!((p->task) && (p->hard == my->is_hard))) {
            all_ok = 0;
        }
    }
    if (all_ok) {
        rt_sem_signal(&p->sem);
    } else {
        long i;
        unsigned long flags;
        struct par_t *par;
        char msg[MAX_MSG_SIZE];
        struct sockaddr *addr;
        par = (void *)msg;
        addr = (struct sockaddr*)&p->addr;
        
        if (my->is_hard) {
            while (hard_rt_recvfrom(p->socket[1], msg, MAX_MSG_SIZE, 0, addr, (void *)&i) == -EAGAIN);
        } else {
            soft_rt_recvfrom(p->socket[0], msg, MAX_MSG_SIZE, 0, addr, &i);
        }
        
        flags = rt_spin_lock_irqsave(&recovery_lock);
        recovery.msg[recovery.in].priority = par->priority;
        recovery.msg[recovery.in].owner = par->owner;
        recovery.msg[recovery.in].addr = *addr;
        recovery.msg[recovery.in].hard = my->is_hard;
        recovery.in = (recovery.in + 1) & MaxStubsMone;
        rt_spin_unlock_irqrestore(flags, &recovery_lock);
        rt_sem_signal(&portslot[0].sem);
    }
}

int get_min_tasks_cpuid(void);
int set_rtext(RT_TASK *, int, int, void(*)(void), unsigned int, void *);
int clr_rtext(RT_TASK *);
void rt_schedule_soft(RT_TASK *);

static inline int soft_rt_fun_call(RT_TASK *task, void *fun, void *arg)
{
    union { long long ll; long l; } retval;
    task->fun_args[0] = (long)arg;
    ((struct fun_args *)task->fun_args)->fun = fun;
    rt_schedule_soft(task);
    retval.ll = task->retval;
    return (int)retval.l;
}

static inline long long soft_rt_genfun_call(RT_TASK *task, void *fun, void *args, int argsize)
{
    memcpy(task->fun_args, args, argsize);
    ((struct fun_args *)task->fun_args)->fun = fun;
    rt_schedule_soft(task);
    return task->retval;
}

extern void rt_daemonize(void);
static void thread_fun(RT_TASK *task)
{
    if (!set_rtext(task, task->fun_args[3], 0, 0, get_min_tasks_cpuid(), 0)) {
        rt_daemonize();
        sigfillset(&current->blocked);
        rtai_set_linux_task_priority(current, SCHED_FIFO, MIN_LINUX_RTPRIO);
        soft_rt_fun_call(task, rt_task_suspend, task);
        ((void (*)(long))task->fun_args[1])(task->fun_args[2]);
        task->fun_args[1] = 0;
    }
}

static int soft_kthread_init(RT_TASK *task, long fun, long arg, int priority)
{
    task->magic = task->state = 0;
    (task->fun_args = (long *)(task + 1))[1] = fun;
    task->fun_args[2] = arg;
    task->fun_args[3] = priority;
    if (kernel_thread((void *)thread_fun, task, 0) > 0) {
        while (task->state != (RT_SCHED_READY | RT_SCHED_SUSPENDED)) {
            current->state = TASK_INTERRUPTIBLE;
            schedule_timeout(HZ/NETRPC_TIMER_FREQ);
        }
        return 0;
    }
    return -ENOEXEC;
}

static int soft_kthread_delete(RT_TASK *task)
{
    task->fun_args[1] = 1;
    while (task->fun_args[1]) {
        rt_task_masked_unblock(task, ~RT_SCHED_READY);
        current->state = TASK_INTERRUPTIBLE;
        schedule_timeout(HZ/NETRPC_TIMER_FREQ);
    }
        clr_rtext(task);
    return 0;
}

#define ADRSZ  sizeof(struct sockaddr)

static inline int get_stub(unsigned long long owner)
{
    unsigned long flags;
    int i;
    
    flags = rt_spin_lock_irqsave(&stub_lock);
    if (stubssp < MaxStubsMone) {
        struct portslot_t *p;
        i = 1;
        while(((p = portslot[i].p)->owner != owner) && (i < stubssp)) {
            i++;
        }
        if (p->owner != owner) {
            p = portslot[stubssp++].p;
            p->owner = owner;
        }
        rt_spin_unlock_irqrestore(flags, &stub_lock);
        return p->indx;
    }
    rt_spin_unlock_irqrestore(flags, &stub_lock);
    return 0;
}

static inline int gvb_stub(int slot, unsigned long long owner)
{
    unsigned long flags;
    RT_TASK *task;

    flags = rt_spin_lock_irqsave(&stub_lock);
    if (stubssp > 1) {
        if (slot > 0 && slot < MaxStubs) {
            if (portslot[slot].owner == owner) {
                struct portslot_t *tmp_p;
                int tmp_place;
                task = (RT_TASK *)portslot[slot].task;
                portslot[slot].task = 0;
                portslot[slot].owner = 0;
                tmp_p = portslot[--stubssp].p;
                tmp_place = portslot[slot].place;
                portslot[slot].place = stubssp;
                portslot[stubssp].p = &portslot[slot];
                tmp_p->place = tmp_place;
                portslot[tmp_place].p = tmp_p;
                slot += BASEPORT;
                rt_spin_unlock_irqrestore(flags, &stub_lock);
                if (task->is_hard) {
                    rt_task_delete(task);
                } else {
                    soft_kthread_delete(task);
                }
                kfree(task);
            } else {
                slot = !portslot[slot].owner ? slot + BASEPORT : -ENXIO;
                rt_spin_unlock_irqrestore(flags, &stub_lock);
            }
        } else {
            rt_spin_unlock_irqrestore(flags, &stub_lock);
        }       
        return slot;
    }
    rt_spin_unlock_irqrestore(flags, &stub_lock);
    return -EINVAL;
}

static inline int find_stub(unsigned long long owner)
{
    unsigned long flags;
    int i;
    struct portslot_t *p;
    i = 1;
    flags = rt_spin_lock_irqsave(&stub_lock);
    while(((p = portslot[i].p)->owner != owner) && (i < stubssp)) {
        i++;
    }
    rt_spin_unlock_irqrestore(flags, &stub_lock);
    return p->owner != owner ? 0 : p->indx;
}

static void soft_stub_fun(struct portslot_t *portslotp)
{
    char msg[MAX_MSG_SIZE];
    struct sockaddr *addr;
    RT_TASK *task;
    SEM *sem;
        struct par_t *par;
    long wsize, w2size, sock;
    long *ain;
    long type;

    addr = (struct sockaddr *)&portslotp->addr;
    sock = portslotp->socket[0];
    sem  = &portslotp->sem;
    ain = (par = (void *)msg)->a;
    task = (RT_TASK *)portslotp->task;
    sprintf(current->comm, "SFTSTB-%ld", sock);
    
recvrys:

    while (soft_rt_fun_call(task, rt_sem_wait, sem) < RTE_LOWERR) {
        wsize = soft_rt_recvfrom(sock, msg, MAX_MSG_SIZE, 0, addr, &w2size);
        if (decode) {
            decode(portslotp, msg, wsize, RPC_SRV);
        }
        if (portslotp->owner != par->owner) {
            unsigned long flags;
            
            flags = rt_spin_lock_irqsave(&recovery_lock);
            recovery.msg[recovery.in].priority = par->priority;
            recovery.msg[recovery.in].owner = par->owner;
            recovery.msg[recovery.in].addr = *addr;
            recovery.msg[recovery.in].hard = 0;
            recovery.in = (recovery.in + 1) & MaxStubsMone;
            rt_spin_unlock_irqrestore(flags, &recovery_lock);
            rt_sem_signal(&portslot[0].sem);
        } else {
            int argsize; 
            long a[par->argsize/sizeof(long) + 1];
            if(par->priority >= 0 && par->priority < RT_SCHED_LINUX_PRIORITY) {
                if ((wsize = par->priority) < task->priority) {
                    task->priority = wsize;
                    rtai_set_linux_task_priority(task->lnxtsk, task->lnxtsk->policy, wsize >= MAX_LINUX_RTPRIO ? MIN_LINUX_RTPRIO : MAX_LINUX_RTPRIO - wsize);
                }
                task->base_priority = par->base_priority;
            }
            argsize = argconv(ain, a, par->mach, par->argsize, par->partypes);
            type = par->type;
            if (par->rsize) {
                a[USP_RBF1(type) - 1] = (long)((char *)ain + par->argsize);
            }
            if (NEED_TO_W(type)) {
                wsize = USP_WSZ1(type);
                wsize = wsize ? a[wsize - 1] : par->mach; //sizeof(long);
            } else {
                wsize = 0;
            }
            if (NEED_TO_W2ND(type)) {
                w2size = USP_WSZ2(type);
                w2size = w2size ? a[w2size - 1] : par->mach; //sizeof(long);
            } else {
                w2size = 0;
            }
            do {
                struct msg_t { struct reply_t arg; char bufspace[wsize + w2size]; } arg;
                arg.arg.myport = 0;
                if (wsize > 0) {
                    arg.arg.wsize = wsize;
                    a[USP_WBF1(type) - 1] = (long)arg.arg.msg;
                } else {
                    arg.arg.wsize = 0;
                }
                if (w2size > 0) {
                    arg.arg.w2size = w2size;
                    a[USP_WBF2(type) - 1] = (long)(arg.arg.msg + arg.arg.wsize);
                } else {
                    arg.arg.w2size = 0;
                }
#ifndef NETRPC_ALIGN_RTIME
                if ((wsize = TIMED(par->fun_ext_timed) - 1) >= 0) {
#else
                if ((wsize = TIMED(par->fun_ext_timed)) > 0) {
                    wsize += (NETRPC_ALIGN_RTIME(wsize) - 1);
#endif
                    *((long long *)(a + wsize)) = nano2count(*((long long *)(a + wsize)));
                }
                arg.arg.retval = soft_rt_genfun_call(task, rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun, a, argsize);
                soft_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
            } while (0);
        }
    }
    if (portslotp->recovered) {
        struct reply_t arg;
        portslotp->recovered = 0;
        arg.myport = sock + BASEPORT;
        arg.retval = portslotp->owner;
        soft_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct reply_t), RPC_RTR) : sizeof(struct reply_t), 0, addr, ADRSZ);
        goto recvrys;
    }
//  soft_rt_fun_call(task, rt_task_suspend, task);
}

static void hard_stub_fun(struct portslot_t *portslotp)
{
    char msg[MAX_MSG_SIZE];
    struct sockaddr *addr;
    RT_TASK *task;
    SEM *sem;
    struct par_t *par;
    long wsize, w2size, sock;
    long *ain;
    long type;

    addr = (struct sockaddr *)&portslotp->addr;
    sock = portslotp->socket[1];
    sem  = &portslotp->sem;
    ain = (par = (void *)msg)->a;
    task = (RT_TASK *)portslotp->task;
    if (task->lnxtsk) {
        sprintf(current->comm, "HRDSTB-%ld", sock);
    }
    
recvryh:

    while (rt_sem_wait(sem) < RTE_LOWERR) {
        wsize = hard_rt_recvfrom(sock, msg, MAX_MSG_SIZE, 0, addr, (void *)&w2size);
        if (decode) {
            decode(portslotp, msg, wsize, RPC_SRV);
        }
        if (portslotp->owner != par->owner) {
            unsigned long flags;

            flags = rt_spin_lock_irqsave(&recovery_lock);
            recovery.msg[recovery.in].priority = par->priority;
            recovery.msg[recovery.in].owner = par->owner;
            recovery.msg[recovery.in].addr = *addr;
            recovery.msg[recovery.in].hard = 1;
            recovery.in = (recovery.in + 1) & MaxStubsMone;
            rt_spin_unlock_irqrestore(flags, &recovery_lock);
            rt_sem_signal(&portslot[0].sem);
        } else {
            int argsize;
            long a[par->argsize/sizeof(long) + 1];
            if(par->priority >= 0 && par->priority < RT_SCHED_LINUX_PRIORITY) {
                if ((wsize = par->priority) < task->priority) {
                    task->priority = wsize;
                }
                task->base_priority = par->base_priority;
            }
            argsize = argconv(ain, a, par->mach, par->argsize, par->partypes);
            type = par->type;
            if (par->rsize) {
                a[USP_RBF1(type) - 1] = (long)((char *)ain + par->argsize);
            }
            if (NEED_TO_W(type)) {
                wsize = USP_WSZ1(type);
                wsize = wsize ? a[wsize - 1] : par->mach; //sizeof(long);
            } else {
                wsize = 0;
            }
            if (NEED_TO_W2ND(type)) {
                w2size = USP_WSZ2(type);
                w2size = w2size ? a[w2size - 1] : par->mach; //sizeof(long);
            } else {
                w2size = 0;
            }
            do {
                struct msg_t { struct reply_t arg; char bufspace[wsize + w2size]; } arg;
                arg.arg.myport = 0;
                if (wsize > 0) {
                    arg.arg.wsize = wsize;
                    a[USP_WBF1(type) - 1] = (long)arg.arg.msg;
                } else {
                    arg.arg.wsize = 0;
                }
                if (w2size > 0) {
                    arg.arg.w2size = w2size;
                    a[USP_WBF2(type) - 1] = (long)(arg.arg.msg + arg.arg.wsize);
                } else {
                    arg.arg.w2size = 0;
                }
#ifndef NETRPC_ALIGN_RTIME
                if ((wsize = TIMED(par->fun_ext_timed) - 1) >= 0) {
#else
                if ((wsize = TIMED(par->fun_ext_timed)) > 0) {
                    wsize += (NETRPC_ALIGN_RTIME(wsize) - 1);
#endif
                    *((long long *)(a + wsize)) = nano2count(*((long long *)(a + wsize)));
                }
                arg.arg.retval = ((long long (*)(long, ...))rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun)(RTAI_FUN_A);
                hard_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
            } while (0);
        }
    }
    if (portslotp->recovered) {
        struct reply_t arg;
        portslotp->recovered = 0;
        arg.myport = sock + BASEPORT;
        arg.retval = portslotp->owner;
        hard_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct reply_t), RPC_RTR) : sizeof(struct reply_t), 0, addr, ADRSZ);
        goto recvryh;
    }
    rt_task_suspend(task);
}

static void port_server_fun(RT_TASK *port_server)
{
    short recovered;
    long i, rsize;
    RT_TASK *task;
    struct sockaddr *addr;
    struct req_rel_msg msg;

    addr = (struct sockaddr *)&portslot[0].addr;
    sprintf(current->comm, "PRTSRV");

    while (soft_rt_fun_call(port_server, rt_sem_wait, &portslot[0].sem) < RTE_LOWERR) {
        if (recovery.out != recovery.in) {
            recovered = 1;
            msg.priority = recovery.msg[recovery.out].priority;
            msg.hard = recovery.msg[recovery.out].hard;
            msg.owner = recovery.msg[recovery.out].owner;
            msg.name = TSK_FRM_WNR(recovery.msg[recovery.out].owner);
            *addr = recovery.msg[recovery.out].addr;
            msg.op = 0;
            msg.port = 0;
            recovery.out = (recovery.out + 1) & MaxStubsMone;
        } else {
            recovered = 0;
            rsize = soft_rt_recvfrom(portslot[0].socket[0], &msg, sizeof(msg), 0, addr, &i);
            if (decode) {
                decode(&portslot[0], &msg, rsize, PRT_SRV);
            }
        }
        if (msg.op) {
            msg.port = gvb_stub(msg.op - BASEPORT, msg.owner);
            goto ret;
        }
        if (!(msg.port = get_stub(msg.owner))) {
            msg.port = -ENODEV;
            goto ret;
        }
        if (!portslot[msg.port].task) {
            if ((task = kmalloc(sizeof(RT_TASK) + 2*sizeof(struct fun_args), GFP_KERNEL))) {
                if ((msg.hard ? rt_task_init(task, (void *)hard_stub_fun, (long)(portslot + msg.port), StackSize + 2*MAX_MSG_SIZE, msg.priority, 0, NULL) : soft_kthread_init(task, (long)soft_stub_fun, (long)(portslot + msg.port), msg.priority < BASE_SOFT_PRIORITY ? msg.priority + BASE_SOFT_PRIORITY : msg.priority))) {
                    kfree(task);
                    task = 0;
                }
            }
            if (!task) {
                portslot[msg.port].owner = 0;
                msg.port = -ENOMEM;
                goto ret;
            }
            portslot[msg.port].name = msg.name;
            portslot[msg.port].task = (long)task;
            if (msg.hard) { 
                portslot[msg.port].hard = MSG_HARD;
            } else {
                portslot[msg.port].hard = MSG_SOFT;
            }
            portslot[msg.port].sem.count = 0;
            portslot[msg.port].sem.queue.prev = portslot[msg.port].sem.queue.next = &portslot[msg.port].sem.queue;
            portslot[msg.port].addr = portslot[0].addr;
            rt_task_resume(task);
        }
        portslot[msg.port].recovered = recovered;
        msg.rem_node = this_node[msg.hard];
        msg.port += BASEPORT;
        msg.chkspare = sizeof(long);
ret:
        if (recovered) {
            rt_task_masked_unblock((RT_TASK *)portslot[msg.port-BASEPORT].task,~RT_SCHED_READY);
        } else {
            soft_rt_sendto(portslot[0].socket[0], &msg, encode ? encode(&portslot[0], &msg, sizeof(msg), PRT_RTR) : sizeof(msg), 0, addr, ADRSZ);
        }
    }
//soft_rt_fun_call(port_server, rt_task_suspend, port_server);
}

static int mod_timer_srq;

RTAI_SYSCALL_MODE int rt_send_req_rel_port(unsigned long node, int op, unsigned long id, MBX *mbx, int hard)
{
    RT_TASK *task;
    long i, msgsize;
    struct portslot_t *portslotp;
    struct req_rel_msg msg;

    op >>= PORT_SHF;
    if (!node || (op && (op < MaxStubs || op >= MaxSocks))) {
        return -EINVAL;
    }
    if (!(portslotp = get_portslot())) {
        return -ENODEV;
    }
    portslotp->name = PRTSRVNAME;
    portslotp->addr = SPRT_ADDR;
    portslotp->addr.sin_addr.s_addr = node;
    task = _rt_whoami();
    if (op) {
        msg.op = ntohs(portslot[op].addr.sin_port);
        id = portslot[op].name;
        hard = portslot[op].hard;
    } else {
        msg.op = 0;
        if (!id) {
            id = (unsigned long)task;
        }
    }
    msg.port = portslotp->sem.count = 0;
    portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
    msg.hard = hard ? MSG_HARD : MSG_SOFT;
    msg.name = id;
    msg.owner = OWNER(this_node[0], id);
    msg.rem_node = 0;
    msg.priority = task->base_priority;
    msgsize = encode ? encode(&portslot[0], &msg, sizeof(msg), PRT_REQ) : sizeof(msg);
    for (i = 0; i < NETRPC_TIMER_FREQ && !portslotp->sem.count; i++) {
        soft_rt_sendto(portslotp->socket[0], &msg, msgsize, 0, (void *)&portslotp->addr, ADRSZ);
        rt_pend_linux_srq(mod_timer_srq);
        rt_sem_wait(&timer_sem);
    }
    if (portslotp->sem.count >= 1) {
        msgsize = soft_rt_recvfrom(portslotp->socket[0], &msg, sizeof(msg), 0, (void *)&portslotp->addr, &i);   
        if (decode) {
            decode(&portslot[0], &msg, msgsize, PRT_RCV);
        }
        if (msg.port > 0) {
            if (op) {
                portslot[op].task = 0;
                gvb_portslot(portslot + op);
                gvb_portslot(portslotp);
                return op;
            } else {
                check_portslot(node, msg.port, &portslotp);
                portslotp->sem.count = 0;
                portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
                portslotp->hard = msg.hard;
                portslotp->owner = msg.owner;
                portslotp->name = msg.name;
                portslotp->addr.sin_port = htons(msg.port);
                portslotp->mbx  = mbx;
                portslotp->recovered = 1;
                portslotp->addr.sin_addr.s_addr = msg.rem_node; 
                if (msg.chkspare == 4) {
                    return (portslotp->indx << PORT_SHF);
                } else {
                    return (portslotp->indx << PORT_SHF) + PORT_INC;
                }
            }
        }
    }
    gvb_portslot(portslotp);
    return msg.port ? msg.port : -ETIMEDOUT;
}

RTAI_SYSCALL_MODE int rt_set_netrpc_timeout(int port, RTIME timeout)
{
    portslot[port >> PORT_SHF].timeout = timeout;
    return 0;
}

RTAI_SYSCALL_MODE RT_TASK *rt_find_asgn_stub(unsigned long long owner, int asgn)
{
    int i;
    i = asgn ? get_stub(owner) : find_stub(owner);
    return i > 0 ? (RT_TASK *)portslot[i].task : 0;
}

RTAI_SYSCALL_MODE int rt_rel_stub(unsigned long long owner)
{
    int i;
    i = find_stub(owner);
    if (i)
    {
    i = gvb_stub(i,owner);  
    return i;
    } else {
    return -ESRCH;
    }
}

RTAI_SYSCALL_MODE int rt_waiting_return(unsigned long node, int port)
{
    struct portslot_t *portslotp;
    portslotp = portslot + (abs(port) >> PORT_SHF);
    return portslotp->task < 0 && !portslotp->sem.count;
}

#if 0

static inline void mbx_send_if(MBX *mbx, void *sendmsg, int msg_size)
{
#define MOD_SIZE(indx) ((indx) < mbx->size ? (indx) : (indx) - mbx->size)

    unsigned long flags;
    int tocpy, avbs;
    char *msg;

    if (!mbx) {
        return;
    }
    msg = sendmsg;
    if (msg_size <= mbx->frbs) {
        RT_TASK *task;
        avbs = mbx->avbs;
        while (msg_size > 0 && mbx->frbs) {
            if ((tocpy = mbx->size - mbx->lbyte) > msg_size) {
                tocpy = msg_size;
            }
            if (tocpy > mbx->frbs) {
                tocpy = mbx->frbs;
            }
            memcpy(mbx->bufadr + mbx->lbyte, msg, tocpy);
            flags = rt_spin_lock_irqsave(&mbx->lock);
            mbx->frbs -= tocpy;
            rt_spin_unlock_irqrestore(flags, &mbx->lock);
            avbs += tocpy;
            msg_size -= tocpy;
            *msg += tocpy;
            mbx->lbyte = MOD_SIZE(mbx->lbyte + tocpy);
        }
        mbx->avbs = avbs;
        flags = rt_global_save_flags_and_cli();
        if ((task = mbx->waiting_task)) {
            rem_timed_task(task);
            mbx->waiting_task = (void *)0;
                    if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_MBXSUSP | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
                            enq_ready_task(task);
                rt_schedule();
                }
        }
        rt_global_restore_flags(flags);
    }
}

#else

static inline void mbx_signal(MBX *mbx)
{
    unsigned long flags;
    RT_TASK *task;

    flags = rt_global_save_flags_and_cli();
    if ((task = mbx->waiting_task)) {
        rem_timed_task(task);
        task->blocked_on  = NULL;
        mbx->waiting_task = NULL;
        if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_MBXSUSP | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
            enq_ready_task(task);
            RT_SCHEDULE(task, rtai_cpuid());
        }
    }
    rt_global_restore_flags(flags);
}

#define MOD_SIZE(indx) ((indx) < mbx->size ? (indx) : (indx) - mbx->size)

static inline void mbxput(MBX *mbx, char **msg, int msg_size)
{
    unsigned long flags;
    int tocpy;

    while (msg_size > 0 && mbx->frbs) {
        if ((tocpy = mbx->size - mbx->lbyte) > msg_size) {
            tocpy = msg_size;
        }
        if (tocpy > mbx->frbs) {
            tocpy = mbx->frbs;
        }
        memcpy(mbx->bufadr + mbx->lbyte, *msg, tocpy);
        flags = rt_spin_lock_irqsave(&(mbx->lock));
        mbx->frbs -= tocpy;
        mbx->avbs += tocpy;
        rt_spin_unlock_irqrestore(flags, &(mbx->lock));
        msg_size -= tocpy;
        *msg     += tocpy;
        mbx->lbyte = MOD_SIZE(mbx->lbyte + tocpy);
    }
}

static void mbx_send_if(MBX *mbx, void *msg, int msg_size)
{
    unsigned long flags;
    RT_TASK *rt_current;

    if (!mbx || mbx->magic != RT_MBX_MAGIC) {
        return;
    }

    flags = rt_global_save_flags_and_cli();
    rt_current = RT_CURRENT;
    if (mbx->sndsem.count > 0 && msg_size <= mbx->frbs) {
        mbx->sndsem.count = 0;
        if (mbx->sndsem.type > 0) {
            mbx->sndsem.owndby = rt_current;
            enqueue_resqel(&mbx->sndsem.resq, rt_current);
        }
        rt_global_restore_flags(flags);
        mbxput(mbx, (char **)(&msg), msg_size);
        mbx_signal(mbx);
        rt_sem_signal(&mbx->sndsem);
    }
    rt_global_restore_flags(flags);
}

#endif

#define RETURN_ERR(err) \
    do { \
        union { long long ll; long l; } retval; \
        retval.l = err; \
        return retval.ll; \
    } while (0)

RTAI_SYSCALL_MODE long long _rt_net_rpc(long fun_ext_timed, long type, void *args, int argsize, int space, unsigned long partypes)
{
    char msg[MAX_MSG_SIZE];
    struct reply_t *reply;
    long rsize, port;
    struct portslot_t *portslotp;

    if ((port = PORT(fun_ext_timed)) > 0) {
        port >>= PORT_SHF;
        if ((portslotp = portslot + port)->task < 0) {
            long i;
            struct sockaddr addr;
            
            if (portslotp->timeout) {
                if(rt_sem_wait_timed(&portslotp->sem,portslotp->timeout) == RTE_TIMOUT)
                    RETURN_ERR(RTE_NETIMOUT);
            } else {
                rt_sem_wait(&portslotp->sem);
            }
            if ((rsize = portslotp->hard ? hard_rt_recvfrom(portslotp->socket[1], msg, MAX_MSG_SIZE, 0, &addr, (void *)&i) : soft_rt_recvfrom(portslotp->socket[0], msg, MAX_MSG_SIZE, 0, &addr, &i))) {
                if (decode) {
                    rsize = decode(portslotp, msg, rsize, RPC_RCV);
                }
                if((reply = (void *)msg)->myport) {
                    if (reply->myport < 0) {
                        RETURN_ERR(-RTE_CHGPORTERR);    
                    }
                    portslotp->addr.sin_port = htons(reply->myport);
                    portslotp->sem.count = 0;
                    portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
                    portslotp->owner = reply->retval;
                    portslotp->name = (unsigned long)(_rt_whoami());
                    RETURN_ERR(-RTE_CHGPORTOK);
                }
                mbx_send_if(portslotp->mbx, msg, offsetof(struct reply_t, msg) + reply->wsize + reply->w2size);
//              mbx_send_if(portslotp->mbx, msg, rsize);
            }
            portslotp->task = 1;
        }
        portslotp->msg = msg;
    } else {
        port = -(abs(port) >> PORT_SHF);
        if ((portslotp = portslot - port)->task < 0) {
            if (!rt_sem_wait_if(&portslotp->sem)) {
                return 0;
            } else {
                long i;
                struct sockaddr addr;
                
                if ((rsize = portslotp->hard ? hard_rt_recvfrom(portslotp->socket[1], msg, MAX_MSG_SIZE, 0, &addr, (void *)&i) : soft_rt_recvfrom(portslotp->socket[0], msg, MAX_MSG_SIZE, 0, &addr, &i))) {
                    if (decode) {
                        rsize = decode(portslotp, msg, rsize, RPC_RCV);
                    }
                        if((reply = (void *)msg)->myport) {
                            if (reply->myport < 0) {
                                RETURN_ERR(-RTE_CHGPORTERR);
                            }

                                portslotp->addr.sin_port = htons(reply->myport);
                                portslotp->sem.count = 0;
                                portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
                                portslotp->owner = reply->retval;
                                portslotp->name = (unsigned long)(_rt_whoami());
                                RETURN_ERR(-RTE_CHGPORTOK);
                        }
                    mbx_send_if(portslotp->mbx, msg, offsetof(struct reply_t, msg) + reply->wsize + reply->w2size);
//                  mbx_send_if(portslotp->mbx, msg, rsize);
                }
            }
        } else {
            portslotp->task = -1;
        }
    }
    if (FUN(fun_ext_timed) == SYNC_NET_RPC) {
        RETURN_ERR(1);
    }
    if (NEED_TO_R(type)) {
        rsize = USP_RSZ1(type);
        rsize = rsize ? ((long *)args)[rsize - 1] : sizeof(long);
    } else {
        rsize = 0;
    }
    do {
        struct par_t *arg;
        RT_TASK *task;

        arg = (void *)msg;
        arg->priority = (task = _rt_whoami())->priority;
        arg->base_priority = task->base_priority;
        arg->argsize = argsize;
        arg->rsize = rsize;
        arg->fun_ext_timed = fun_ext_timed;
        arg->type = type;
        arg->owner = portslotp->owner;
        arg->partypes = partypes;
        memcpy(arg->a, args, argsize);
        if (rsize > 0) {
            if (space) {
                memcpy((char *)arg->a + argsize, (void *)((long *)args + USP_RBF1(type) - 1)[0], rsize);
            } else {
                rt_copy_from_user((char *)arg->a + argsize, (void *)((long *)args + USP_RBF1(type) - 1)[0], rsize);
            }
        }
        rsize = sizeof(struct par_t) - sizeof(long) + argsize + rsize;
        if (encode) {
            rsize = encode(portslotp, msg, rsize, RPC_REQ);
        }
        arg->mach = sizeof(long);
        if (portslotp->hard) {
            hard_rt_sendto(portslotp->socket[1], msg, rsize, 0, (struct sockaddr *)&portslotp->addr, ADRSZ);
        } else  {
            soft_rt_sendto(portslotp->socket[0], msg, rsize, 0, (struct sockaddr *)&portslotp->addr, ADRSZ);
        }
    } while (0);
    if (port > 0) {
        struct sockaddr addr;

        if (portslotp->timeout) {
            if(rt_sem_wait_timed(&portslotp->sem,portslotp->timeout) == RTE_TIMOUT)
                RETURN_ERR(-RTE_NETIMOUT);
        } else {
            rt_sem_wait(&portslotp->sem);
        }
        rsize = portslotp->hard ? hard_rt_recvfrom(portslotp->socket[1], msg, MAX_MSG_SIZE, 0, &addr, (void *)&port) : soft_rt_recvfrom(portslotp->socket[0], msg, MAX_MSG_SIZE, 0, &addr, &port);
        if (decode) {
            decode(portslotp, portslotp->msg, rsize, RPC_RCV);
        }
        if((reply = (void *)msg)->myport) {
            if (reply->myport < 0) {
                RETURN_ERR(-RTE_CHGPORTERR);    
            }
            portslotp->addr.sin_port = htons(reply->myport);
            portslotp->sem.count = 0;
            portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
            portslotp->owner = reply->retval;
            portslotp->name = (unsigned long)(_rt_whoami());
            RETURN_ERR(-RTE_CHGPORTOK);
        } else {
            if (reply->wsize) {
                if (space) {
                    memcpy((char *)(*((long *)args + USP_WBF1(type) - 1)), reply->msg, reply->wsize);
                } else {
                    rt_copy_to_user((char *)(*((long *)args + USP_WBF1(type) - 1)), reply->msg, reply->wsize);
                }
                if (reply->w2size) {
                    if (space) {
                        memcpy((char *)(*((long *)args + USP_WBF2(type) - 1)), reply->msg + reply->wsize, reply->w2size);
                    } else {
                        rt_copy_to_user((char *)(*((long *)args + USP_WBF2(type) - 1)), reply->msg + reply->wsize, reply->w2size);
                    }
                }
            }

            return reply->retval;
        }
    }
    return 0;
}

int rt_get_net_rpc_ret(MBX *mbx, unsigned long long *retval, void *msg1, int *msglen1, void *msg2, int *msglen2, RTIME timeout, int type)
{
    struct reply_t reply;
    int ret;

    if ((ret = ((int (*)(MBX *, ...))rt_net_rpc_fun_ext[NET_RPC_EXT][type].fun)(mbx, &reply, offsetof(struct reply_t, msg), timeout))) {
        return ret;
    }
    *retval = reply.retval;
    if (reply.wsize) {
        if (*msglen1 > reply.wsize) {
            *msglen1 = reply.wsize;
        }
        _rt_mbx_receive(mbx, &msg1, *msglen1, 1);
    } else {
        *msglen1 = 0;
    }
    if (reply.w2size) {
        if (*msglen2 > reply.w2size) {
            *msglen2 = reply.w2size;
        }
        _rt_mbx_receive(mbx, &msg2, *msglen2, 1);
    } else {
        *msglen2 = 0;
    }
    return 0;
}

RTAI_SYSCALL_MODE unsigned long ddn2nl(const char *ddn)
{
    int p, n, c;
    union { unsigned long l; char c[4]; } u;

    p = n = 0;
    while ((c = *ddn++)) {
        if (c != '.') {
            n = n*10 + c - '0';
        } else {
            if (n > 0xFF) {
                return 0;
            }
            u.c[p++] = n;
            n = 0;
        }
    }
    u.c[3] = n;

    return u.l;
}

RTAI_SYSCALL_MODE unsigned long rt_set_this_node(const char *ddn, unsigned long node, int hard)
{
    return this_node[hard ? MSG_HARD : MSG_SOFT] = ddn ? ddn2nl(ddn) : node;
}

#ifdef CONFIG_RTAI_RT_POLL

RTAI_SYSCALL_MODE int rt_poll_netrpc(struct rt_poll_s *pdsa1, struct rt_poll_s *pdsa2, unsigned long pdsa_size, RTIME timeout)
{
    int retval = pdsa_size/sizeof(struct rt_poll_s);
    if (sizeof(long) == 8 && !((unsigned long)pdsa1[0].what & 0xFFFFFFFF00000000ULL)) {
        int i;
        for (i = 0; i < retval; i++) {
            pdsa1[i].what = (void *)reset_kadr((unsigned long)pdsa1[i].what);
        }
    }
    retval = _rt_poll(pdsa1, retval, timeout, 1);
    memcpy(pdsa2, pdsa1, pdsa_size);
    return retval;
}

EXPORT_SYMBOL(rt_poll_netrpc);

#endif

/* +++++++++++++++++++++++++++ NETRPC ENTRIES +++++++++++++++++++++++++++++++ */

struct rt_native_fun_entry rt_netrpc_entries[] = {
    { { 1, _rt_net_rpc           },  NETRPC },
    { { 0, rt_set_netrpc_timeout },  SET_NETRPC_TIMEOUT },
    { { 1, rt_send_req_rel_port  },  SEND_REQ_REL_PORT },
    { { 0, ddn2nl                },  DDN2NL },
    { { 0, rt_set_this_node      },  SET_THIS_NODE },
    { { 0, rt_find_asgn_stub     },  FIND_ASGN_STUB },
    { { 0, rt_rel_stub           },  REL_STUB },
    { { 0, rt_waiting_return     },  WAITING_RETURN },
#ifdef CONFIG_RTAI_RT_POLL
    { { 1, rt_poll_netrpc        },  RT_POLL_NETRPC },
#endif
    { { 0, 0 },                     000 }
};

extern int set_rt_fun_entries(struct rt_native_fun_entry *entry);
extern void reset_rt_fun_entries(struct rt_native_fun_entry *entry);

static RT_TASK *port_server;

static int init_softrtnet(void);
static void cleanup_softrtnet(void);

void do_mod_timer(void)
{
    mod_timer(&timer, jiffies + HZ/NETRPC_TIMER_FREQ);
}

static struct sock_t *socks;

int soft_rt_socket(int domain, int type, int protocol)
{
    int i;
    for (i = 0; i < MaxSocks; i++) {
        if (!cmpxchg(&socks[i].opnd, 0, 1)) {
            return i;
        }
    }
    return -1;
}

int soft_rt_close(int sock)
{
    if (sock >= 0 && sock < MaxSocks) {
        return socks[sock].opnd = 0;
    }
    return -1;
}

int soft_rt_bind(int sock, struct sockaddr *addr, int addrlen)
{
    return 0;
}

int soft_rt_socket_callback(int sock, int (*func)(int sock, void *arg), void *arg)
{
    if (sock >= 0 && sock < MaxSocks && func > 0) {
        socks[sock].callback = func;
        socks[sock].arg      = arg;
        return 0;
    }
    return -1;
}

static int MaxSockSrq;
static struct { int srq, in, out, *sockindx; } sysrq;
static spinlock_t sysrq_lock = SPIN_LOCK_UNLOCKED;

int soft_rt_sendto(int sock, const void *msg, int msglen, unsigned int sflags, struct sockaddr *to, int tolen)
{
    unsigned long flags;
    if (sock >= 0 && sock < MaxSocks) {
        if (msglen > MAX_MSG_SIZE) {
            msglen = MAX_MSG_SIZE;
        }
        memcpy(socks[sock].msg, msg, socks[sock].tosend = msglen);
        memcpy(&socks[sock].addr, to, tolen);
        flags = rt_spin_lock_irqsave(&sysrq_lock);
        sysrq.sockindx[sysrq.in] = sock;
            sysrq.in = (sysrq.in + 1) & MaxSockSrq;
        rt_spin_unlock_irqrestore(flags, &sysrq_lock);
        rt_pend_linux_srq(sysrq.srq);
        return msglen;
    }
    return -1;
}

int soft_rt_recvfrom(int sock, void *msg, int msglen, unsigned int flags, struct sockaddr *from, long *fromlen)
{
    if (sock >= 0 && sock < MaxSocks) {
        if (msglen > socks[sock].recvd) {
            msglen = socks[sock].recvd;
        }
        memcpy(msg, socks[sock].msg, msglen);
        if (from && fromlen) {
            memcpy(from, &socks[sock].addr, socks[sock].addrlen);
            *fromlen = socks[sock].addrlen;
        }
        return msglen;
    }
    return -1;
}

#include <linux/unistd.h>
#include <linux/poll.h>
#include <linux/net.h>

int errno;

#define SYSCALL_BGN() \
    do { int retval; mm_segment_t svdfs = get_fs(); set_fs(KERNEL_DS)
#define SYSCALL_END() \
    set_fs(svdfs); return retval; } while (0)

#ifdef __NR_socketcall

extern void *sys_call_table[];

//static _syscall3(int, poll, struct pollfd *, ufds, unsigned int, nfds, int, timeout)
static inline int kpoll(struct pollfd *ufds, unsigned int nfds, int timeout)
{
    SYSCALL_BGN();
    retval = ((asmlinkage int (*)(struct pollfd *, ... ))sys_call_table[__NR_poll])(ufds, nfds, timeout);
//  retval = poll(ufds, nfds, timeout);
    SYSCALL_END();
}

//static _syscall2(int, socketcall, int, call, void *, args)
static inline int ksocketcall(int call, void *args)
{
    SYSCALL_BGN();
    retval = ((asmlinkage int (*)(int, ... ))sys_call_table[__NR_socketcall])(call, args);
//  retval = socketcall(call, args);
    SYSCALL_END();
}

static inline int ksocket(int family, int type, int protocol)
{
    struct { int family; int type; int protocol; } args = { family, type, protocol };
    return ksocketcall(SYS_SOCKET, &args);
}

static inline int kbind(int fd, struct sockaddr *umyaddr, int addrlen)
{
    struct { int fd; struct sockaddr *umyaddr; int addrlen; } args = { fd, umyaddr, addrlen };
    return ksocketcall(SYS_BIND, &args);
}

static inline int kconnect(int fd, struct sockaddr *serv_addr, int addrlen)
{
    struct { int fd; struct sockaddr *serv_addr; int addrlen; } args = { fd, serv_addr, addrlen };
    return ksocketcall(SYS_CONNECT, &args);
}

static inline int klisten(int fd, int backlog)
{
    struct { int fd; int backlog; } args = { fd, backlog };
    return ksocketcall(SYS_LISTEN, &args);
}

static inline int kaccept(int fd, struct sockaddr *upeer_sockaddr, int *upeer_addrlen)
{
    struct { int fd; struct sockaddr *upeer_sockaddr; int *upeer_addrlen; } args = { fd, upeer_sockaddr, upeer_addrlen };
    return ksocketcall(SYS_ACCEPT, &args);
}

static inline int kgetsockname(int fd, struct sockaddr *usockaddr, int *usockaddr_len)
{
    struct { int fd; struct sockaddr *usockaddr; int *usockaddr_len; } args = { fd, usockaddr, usockaddr_len };
    return ksocketcall(SYS_GETSOCKNAME, &args);
}

static inline int kgetpeername(int fd, struct sockaddr *usockaddr, int *usockaddr_len)
{
    struct { int fd; struct sockaddr *usockaddr; int *usockaddr_len; } args = { fd, usockaddr, usockaddr_len };
    return ksocketcall(SYS_GETPEERNAME, &args);
}

static inline int ksocketpair(int family, int type, int protocol, int *usockvec)
{
    struct { int family; int type; int protocol; int *usockvec; } args = { family, type, protocol, usockvec };
    return ksocketcall(SYS_SOCKETPAIR, &args);
}

static inline int ksendto(int fd, void *buff, size_t len, unsigned flags, struct sockaddr *addr, int addr_len)
{
    struct { int fd; void *buff; size_t len; unsigned flags; struct sockaddr *addr; int addr_len; } args = { fd, buff, len, flags, addr, addr_len };
    return ksocketcall(SYS_SENDTO, &args);
}

static inline int krecvfrom(int fd, void *ubuf, size_t len, unsigned flags, struct sockaddr *addr, int *addr_len)
{
    struct { int fd; void *ubuf; size_t len; unsigned flags; struct sockaddr *addr; int *addr_len; } args = { fd, ubuf, len, flags, addr, addr_len };
    return ksocketcall(SYS_RECVFROM, &args);
}

static inline int kshutdown(int fd, int how)
{
    struct { int fd; int how; } args = { fd, how };
    return ksocketcall(SYS_SHUTDOWN, &args);
}

static inline int ksetsockopt(int fd, int level, int optname, void *optval, int optlen)
{
    struct { int fd; int level; int optname; void *optval; int optlen; } args = { fd, level, optname, optval, optlen };
    return ksocketcall(SYS_SETSOCKOPT, &args);
}

static inline int kgetsockopt(int fd, int level, int optname, char *optval, int *optlen)
{
    struct { int fd; int level; int optname; void *optval; int *optlen; } args = { fd, level, optname, optval, optlen };
    return ksocketcall(SYS_GETSOCKOPT, &args);
}

static inline int ksendmsg(int fd, struct msghdr *msg, unsigned flags)
{
    struct { int fd; struct msghdr *msg; unsigned flags; } args = { fd, msg, flags };
    return ksocketcall(SYS_SENDMSG, &args);
}

static inline int krecvmsg(int fd, struct msghdr *msg, unsigned flags)
{
    struct { int fd; struct msghdr *msg; unsigned flags; } args = { fd, msg, flags };
    return ksocketcall(SYS_RECVMSG, &args);
}

#else

#if 0  // just for compiling
#define __NR_socket       1
#define __NR_bind         1
#define __NR_connect      1
#define __NR_accept       1
#define __NR_listen       1
#define __NR_getsockname  1
#define __NR_getpeername  1
#define __NR_socketpair   1
#define __NR_sendto       1
#define __NR_recvfrom     1
#define __NR_shutdown     1
#define __NR_setsockopt   1
#define __NR_getsockopt   1
#define __NR_sendmsg      1
#define __NR_recvmsg      1
#endif

extern void *sys_call_table[];

//static _syscall3(int, poll, struct pollfd *, ufds, unsigned int, nfds, int, timeout)
static inline int kpoll(struct pollfd *ufds, unsigned int nfds, int timeout)
{
    SYSCALL_BGN();
    retval = ((int (*)(void *, unsigned int, int))sys_call_table[__NR_poll])(ufds, nfds, timeout);
//  retval = poll(ufds, nfds, timeout);
    SYSCALL_END();
}

//static _syscall3(int, socket, int, family, int, type, int, protocol)
static inline int ksocket(int family, int type, int protocol)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, int, int))sys_call_table[__NR_socket])(family, type, protocol);
//  retval = socket(family, type, protocol);
    SYSCALL_END();
}

//static _syscall3(int, bind, int, fd, struct sockaddr *, umyaddr, int, addrlen)
static inline int kbind(int fd, struct sockaddr *umyaddr, int addrlen)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct sockaddr *, int))sys_call_table[__NR_bind])(fd, umyaddr, addrlen);
//  retval = bind(fd, umyaddr, addrlen);
    SYSCALL_END();
}

//static _syscall3(int, connect, int, fd, struct sockaddr *, serv_addr, int, addrlen)
static inline int kconnect(int fd, struct sockaddr *serv_addr, int addrlen)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct sockaddr *, int))sys_call_table[__NR_connect])(fd, serv_addr, addrlen);
//  retval = connect(fd, serv_addr, addrlen);
    SYSCALL_END();
}

//static _syscall2(int, listen, int, fd, int, backlog)
static inline int klisten(int fd, int backlog)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, int))sys_call_table[__NR_listen])(fd, backlog);
//  retval = listen(fd, backlog);
    SYSCALL_END();
}

//static _syscall3(int, accept, int, fd, struct sockaddr *, upeer_sockaddr, int *, upeer_addrlen)
static inline int kaccept(int fd, struct sockaddr *upeer_sockaddr, int *upeer_addrlen)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct sockaddr *, int *))sys_call_table[__NR_accept])(fd, upeer_sockaddr, upeer_addrlen);
//  retval = accept(fd, upeer_sockaddr, upeer_addrlen);
    SYSCALL_END();
}

//static _syscall3(int, getsockname, int, fd, struct sockaddr *, usockaddr, int *, uaddr_len)
static inline int kgetsockname(int fd, struct sockaddr *usockaddr, int *usockaddr_len)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct sockaddr *, int *))sys_call_table[__NR_getsockname])(fd, usockaddr, usockaddr_len);
//  retval = getsockname(fd, usockaddr, usockaddr_len);
    SYSCALL_END();
}

//static _syscall3(int, getpeername, int, fd, struct sockaddr *, usockaddr, int *, uaddr_len)
static inline int kgetpeername(int fd, struct sockaddr *usockaddr, int *usockaddr_len)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct sockaddr *, int *))sys_call_table[__NR_getpeername])(fd, usockaddr, usockaddr_len);
//  retval = getpeername(fd, usockaddr, usockaddr_len);
    SYSCALL_END();
}

//static _syscall4(int, socketpair, int, family, int, type, int, protocol, int, usockvec[2])
static inline int ksocketpair(int family, int type, int protocol, int *usockvec)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, int, int, int *))sys_call_table[__NR_socketpair])(family, type, protocol, usockvec);
//  retval = socketpair(family, type, protocol, usockvec);
    SYSCALL_END();
}

//static _syscall6(int, sendto, int, fd, void *, ubuf, size_t, len, unsigned, flags, struct sockaddr *, addr, int, addr_len)
static inline int ksendto(int fd, void *buff, size_t len, unsigned flags, struct sockaddr *addr, int addr_len)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, void *, size_t, unsigned, struct sockaddr *, int))sys_call_table[__NR_sendto])(fd, buff, len, flags, addr, addr_len);
//  retval = sendto(fd, buff, len, flags, addr, addr_len);
    SYSCALL_END();
}

//static _syscall6(int, recvfrom, int, fd, void *, ubuf, size_t, len, unsigned, flags, struct sockaddr *, addr, int *, addr_len)
static inline int krecvfrom(int fd, void *ubuf, size_t len, unsigned flags, struct sockaddr *addr, int *addr_len)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, void *, size_t, unsigned, struct sockaddr *, int *))sys_call_table[__NR_recvfrom])(fd, ubuf, len, flags, addr, addr_len);
//  retval = recvfrom(fd, ubuf, len, flags, addr, addr_len);
    SYSCALL_END();
}

//static _syscall2(int, shutdown, int, fd, int, how)
static inline int kshutdown(int fd, int how)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, int))sys_call_table[__NR_shutdown])(fd, how);
//  retval = shutdown(fd, how);
    SYSCALL_END();
}

//static _syscall5(int, setsockopt, int, fd, int, level, int, optname, void *, optval, int, optlen)
static inline int ksetsockopt(int fd, int level, int optname, void *optval, int optlen)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, int, int, void *, int))sys_call_table[__NR_setsockopt])(fd, level, optname, optval, optlen);
//  retval = setsockopt(fd, level, optname, optval, optlen);
    SYSCALL_END();
}

//static _syscall5(int, getsockopt, int, fd, int, level, int, optname, char *, optval, int *, optlen)
static inline int kgetsockopt(int fd, int level, int optname, char *optval, int *optlen)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, int, int, void *, int *))sys_call_table[__NR_getsockopt])(fd, level, optname, optval, optlen);
//  retval = getsockopt(fd, level, optname, optval, optlen);
    SYSCALL_END();
}

//static _syscall3(int, sendmsg, int, fd, struct msghdr *, msg, unsigned, flags)
static inline int ksendmsg(int fd, struct msghdr *msg, unsigned flags)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct msghdr *, unsigned))sys_call_table[__NR_sendmsg])(fd, msg, flags);
//  retval = sendmsg(fd, msg, flags);
    SYSCALL_END();
}

//static _syscall3(int, recvmsg, int, fd, struct msghdr *, msg, unsigned, flags)
static inline int krecvmsg(int fd, struct msghdr *msg, unsigned flags)
{
    SYSCALL_BGN();
    retval = ((int (*)(int, struct msghdr *, unsigned))sys_call_table[__NR_recvmsg])(fd, msg, flags);
//  retval = recvmsg(fd, msg, flags);
    SYSCALL_END();
}

#endif

static inline int ksend(int fd, void *buff, size_t len, unsigned flags)
{
        return ksendto(fd, buff, len, flags, NULL, 0);
}

static inline int krecv(int fd, void *ubuf, size_t size, unsigned flags)
{
    return krecvfrom(fd, ubuf, size, flags, NULL, NULL);
}

#ifndef DECLARE_MUTEX_LOCKED
#ifndef __DECLARE_SEMAPHORE_GENERIC
#define DECLARE_MUTEX_LOCKED(name) \
    struct semaphore name = __SEMAPHORE_INITIALIZER(name, 0)
#else
#define DECLARE_MUTEX_LOCKED(name) __DECLARE_SEMAPHORE_GENERIC(name,0)
#endif
#endif
static DECLARE_MUTEX_LOCKED(mtx);
static unsigned long end_softrtnet;

static void send_thread(void)
{
    int i;

    sprintf(current->comm, "SNDSRV");
    rtai_set_linux_task_priority(current,SCHED_FIFO,MAX_LINUX_RTPRIO);
    sigfillset(&current->blocked);
    while (!end_softrtnet) {
        i = down_interruptible(&mtx);
        while (sysrq.out != sysrq.in) {
            i = sysrq.sockindx[sysrq.out];
            ksendto(socks[i].sock, socks[i].msg, socks[i].tosend, MSG_DONTWAIT, &socks[i].addr, ADRSZ);
            sysrq.out = (sysrq.out + 1) & MaxSockSrq;
        }
    }
    set_bit(1, &end_softrtnet);
}

static struct pollfd *pollv;
static struct task_struct *recv_handle;

static void recv_thread(void)
{
    int i, nevents;

    recv_handle = current;
    sprintf(current->comm, "RCVSRV");
    rtai_set_linux_task_priority(current,SCHED_RR,MAX_LINUX_RTPRIO);
    sigfillset(&current->blocked);
    while (!end_softrtnet) {
        if ((nevents = kpoll(pollv, MaxSocks, 1000)) > 0) {
            i = -1;
            do {
                while (!pollv[++i].revents);
                if ((socks[i].recvd = krecvfrom(socks[i].sock, socks[i].msg, MAX_MSG_SIZE, MSG_DONTWAIT, &socks[i].addr, &socks[i].addrlen)) > 0) {
                    socks[i].callback(i, socks[i].arg);
                }
            } while (--nevents);
        }
    }
    set_bit(2, &end_softrtnet);
}

static void softrtnet_hdl(void)
{
    up(&mtx);
}

static int init_softrtnet(void)
{
    int i;
    for (i = 8*sizeof(unsigned long) - 1; !test_bit(i, &MaxSocks); i--);
    MaxSockSrq = ((1 << i) != MaxSocks ? 1 << (i + 1) : MaxSocks) - 1;
    if ((sysrq.srq = rt_request_srq(0xbadbeef2, softrtnet_hdl, 0)) < 0) {
        printk("SOFT RTNet: no sysrq available.\n");
        return sysrq.srq;
    }
    if (!(sysrq.sockindx = (int *)kmalloc((MaxSockSrq + 1)*sizeof(int), GFP_KERNEL))) {
        printk("SOFT RTNet: no memory available for socket queus.\n");
        return -ENOMEM;
    }
    if (!(socks = (struct sock_t *)kmalloc(MaxSocks*sizeof(struct sock_t), GFP_KERNEL))) {
        kfree(sysrq.sockindx);
        printk("SOFT RTNet: no memory available for socks.\n");
        return -ENOMEM;
    }
    if (!(pollv = (struct pollfd *)kmalloc(MaxSocks*sizeof(struct pollfd), GFP_KERNEL))) {
        kfree(sysrq.sockindx);
        kfree(socks);
        printk("SOFT RTNet: no memory available for polling.\n");
        return -ENOMEM;
    }
    memset(socks, 0, MaxSocks*sizeof(struct sock_t));
    for (i = 0; i < MaxSocks; i++) {
        SPRT_ADDR.sin_port = htons(BASEPORT + i);
        if ((socks[i].sock = ksocket(AF_INET, SOCK_DGRAM, 0)) < 0 || kbind(socks[i].sock, (struct sockaddr *)&SPRT_ADDR, ADRSZ) < 0) {
            rt_free_srq(sysrq.srq);
            kfree(socks);
            kfree(pollv);
            kfree(sysrq.sockindx);
            printk("SOFT RTNet: unable to set up Linux support sockets.\n");
            return -ESOCKTNOSUPPORT;
        }
        socks[i].addrlen = ADRSZ;
        pollv[i].fd     = socks[i].sock;
        pollv[i].events = POLLIN;
    }
    if (kernel_thread((void *)send_thread, 0, 0) <= 0 || kernel_thread((void *)recv_thread, 0, 0) <= 0) {
            kfree(sysrq.sockindx);
            kfree(socks);
            kfree(pollv);
            printk("SOFT RTNet: unable to set up Linux support kernel threads.\n");
            return -EINVAL;
    }
    while (!recv_handle) {
        current->state = TASK_INTERRUPTIBLE;
        schedule_timeout(HZ/2);
    }
    return 0;
}

static void cleanup_softrtnet(void)
{
    int i;
    rt_free_srq(sysrq.srq);
    end_softrtnet = 1;
/* watch out: dirty trick, but we are sure the thread will do nothing more. */
//  sigemptyset(&recv_handle->blocked);
//  send_sig(SIGKILL, recv_handle, 1);
/* watch out: end of the dirty trick. */
    softrtnet_hdl();
    while (end_softrtnet < 7) {
        current->state = TASK_INTERRUPTIBLE;
        schedule_timeout(HZ/2);
    }
    for (i = 0; i < MaxSocks; i++) {
        kshutdown(socks[i].sock, 2);
    }
    kfree(sysrq.sockindx);
    kfree(socks);
    kfree(pollv);
}

/*
 * this is a thing to make available externally what it should not,
 * needed to check the working of a user message processing addon
 */
void **rt_net_rpc_fun_hook = (void *)rt_net_rpc_fun_ext;

int __rtai_netrpc_init(void)
{
    int i;

    MaxStubsMone = MaxStubs - 1;
    if ((mod_timer_srq = rt_request_srq(0xbadbeef1, do_mod_timer, 0)) < 0) {
        printk("MOD_TIMER: no sysrq available.\n");
        return mod_timer_srq;
    }
    if (!(recovery.msg = (struct recovery_msg *)kmalloc((MaxStubs)*sizeof(struct recovery_msg), GFP_KERNEL))) {
        printk("Init MODULE no memory for recovery queue.\n");
        rt_free_srq(mod_timer_srq);
        return -ENOMEM;
    }
    MaxSocks += MaxStubs;
    SPRT_ADDR.sin_family = AF_INET;
    SPRT_ADDR.sin_addr.s_addr = htonl(INADDR_ANY);
    if (init_softrtnet()) {
        return 1;
    }
    rt_net_rpc_fun_ext[NET_RPC_EXT] = rt_fun_lxrt;
    set_rt_fun_entries(rt_netrpc_entries);
    if (!(portslot = kmalloc(MaxSocks*sizeof(struct portslot_t), GFP_KERNEL))) {
        printk("KMALLOC FAILED ALLOCATING PORT SLOTS\n");
    }
    if (!ThisSoftNode) {
        ThisSoftNode = ThisNode;
    }
    if (!ThisHardNode) {
        ThisHardNode = ThisNode;
    }
    this_node[0] = ddn2nl(ThisSoftNode);
    this_node[1] = ddn2nl(ThisHardNode);

    for (i = 0; i < MaxSocks; i++) {
        portslot[i].p = portslot + i;
        portslot[i].indx = portslot[i].place = i;
        SPRT_ADDR.sin_port = htons(BASEPORT + i);
        portslot[i].addr = SPRT_ADDR;
        portslot[i].socket[0] = soft_rt_socket(AF_INET, SOCK_DGRAM, 0);
        soft_rt_bind(portslot[i].socket[0], (struct sockaddr *)&SPRT_ADDR, ADRSZ);
        portslot[i].socket[1] = hard_rt_socket(AF_INET, SOCK_DGRAM, 0);
        hard_rt_bind(portslot[i].socket[1], (struct sockaddr *)&SPRT_ADDR, ADRSZ);
        soft_rt_socket_callback(portslot[i].socket[0], (void *)net_resume_task, &portslot[i].p);
        hard_rt_socket_callback(portslot[i].socket[1], (void *)net_resume_task, &portslot[i].p);
        portslot[i].owner = 0;
        rt_typed_sem_init(&portslot[i].sem, 0, BIN_SEM | FIFO_Q);
        portslot[i].task = 0;
        portslot[i].timeout = 0;
    }
    SPRT_ADDR.sin_port = htons(BASEPORT);
    portslotsp = MaxStubs;
    portslot[0].hard = 0;
    portslot[0].name = PRTSRVNAME;
    portslot[0].owner = OWNER(this_node[0], (unsigned long)port_server);
    port_server = kmalloc(sizeof(RT_TASK) + 3*sizeof(struct fun_args), GFP_KERNEL);
    soft_kthread_init(port_server, (long)port_server_fun, (long)port_server, RT_SCHED_LOWEST_PRIORITY);
    portslot[0].task = (long)port_server;
    rt_task_resume(port_server);
    rt_typed_sem_init(&timer_sem, 0, BIN_SEM | FIFO_Q);
    init_timer(&timer);
    timer.function = timer_fun;
    return 0 ;
}

void __rtai_netrpc_exit(void)
{
    int i;

    reset_rt_fun_entries(rt_netrpc_entries);
    del_timer(&timer);
    rt_sem_delete(&timer_sem);
    for (i = 0; i < MaxStubs; i++) {
        if (portslot[i].task) {
            if (portslot[i].hard) {
                rt_task_delete((RT_TASK *)portslot[i].task);
            } else {
                soft_kthread_delete((RT_TASK *)portslot[i].task);
                kfree((RT_TASK *)portslot[i].task);
            }
        }
    }
    for (i = 0; i < MaxSocks; i++) {
        rt_sem_delete(&portslot[i].sem);
        soft_rt_close(portslot[i].socket[0]);
        hard_rt_close(portslot[i].socket[1]);
    }
    kfree(portslot);
    cleanup_softrtnet();
    rt_free_srq(mod_timer_srq);
    return;
}

#ifndef CONFIG_RTAI_NETRPC_BUILTIN
module_init(__rtai_netrpc_init);
module_exit(__rtai_netrpc_exit);
#endif /* !CONFIG_RTAI_NETRPC_BUILTIN */

EXPORT_SYMBOL(set_netrpc_encoding);
EXPORT_SYMBOL(rt_send_req_rel_port);
EXPORT_SYMBOL(rt_find_asgn_stub);
EXPORT_SYMBOL(rt_rel_stub);
EXPORT_SYMBOL(rt_waiting_return);
EXPORT_SYMBOL(_rt_net_rpc);
EXPORT_SYMBOL(rt_get_net_rpc_ret);
EXPORT_SYMBOL(rt_set_this_node);
EXPORT_SYMBOL(rt_set_netrpc_timeout);


#ifdef SOFT_RTNET
EXPORT_SYMBOL(soft_rt_socket);
EXPORT_SYMBOL(soft_rt_close);
EXPORT_SYMBOL(soft_rt_bind);
EXPORT_SYMBOL(soft_rt_socket_callback);
EXPORT_SYMBOL(soft_rt_sendto);
EXPORT_SYMBOL(soft_rt_recvfrom);
EXPORT_SYMBOL(ddn2nl);
#endif /* SOFT_RTNET */

EXPORT_SYMBOL(rt_net_rpc_fun_hook);

---