netrpc.c

rtai-3.1-test3的源代码(Real-Time Application Interface )

/*
 * Copyright (C) 1999-2003 Paolo Mantegazza <[e-mail]>
 *
 * 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/config.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_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 "rtnetP.h"

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

#define HARD_RTNET      0

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

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

#if SOFT_RTNET && !HARD_RTNET
#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

#if !SOFT_RTNET && HARD_RTNET
#ifndef COMPILE_ANYHOW
#include <rtnet.h>  // must be the true RTNet header file
#endif
#define MSG_SOFT 1
#define MSG_HARD 1
#define soft_rt_socket           rt_socket
#define soft_rt_bind(a, b, c)    rt_bind(a, b, c)
#define soft_rt_close(a)         rt_close(a)
#define soft_rt_socket_callback  rt_socket_callback
#define soft_rt_recvfrom         rt_recvfrom
#define soft_rt_sendto           rt_sendto
#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  rt_socket_callback
#define hard_rt_recvfrom         rt_recvfrom
#define hard_rt_sendto           rt_sendto
#endif

#if SOFT_RTNET && 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_socket
#define hard_rt_bind             rt_bind
#define hard_rt_close            rt_close
#define hard_rt_socket_callback  rt_socket_callback
#define hard_rt_recvfrom         rt_recvfrom
#define hard_rt_sendto           rt_sendto
#endif

#define LOCALHOST         "127.0.0.1"
#define BASEPORT           5000
#define NETRPC_STACK_SIZE  6000

static unsigned long MaxStubs = MAX_STUBS;
MODULE_PARM(MaxStubs, "i");
static int MaxStubsMone;

static unsigned long MaxSocks = MAX_SOCKS;
MODULE_PARM(MaxSocks, "i");

static int StackSize = NETRPC_STACK_SIZE;
MODULE_PARM(StackSize, "i");

static char *ThisNode = LOCALHOST;
MODULE_PARM(ThisNode, "s");

static char *ThisSoftNode = 0;
MODULE_PARM(ThisSoftNode, "s");

static char *ThisHardNode = 0;
MODULE_PARM(ThisHardNode, "s");

#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; int indx, socket[2], task, hard; unsigned long long owner; SEM sem; void *msg; struct sockaddr_in addr; MBX *mbx; unsigned long name; };
static spinlock_t portslot_lock = SPIN_LOCK_UNLOCKED;
static volatile int portslotsp;
static struct portslot_t *portslot;
static struct sockaddr_in SPRT_ADDR;

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) {
		portslot[--portslotsp].p = portslotp;
		rt_spin_unlock_irqrestore(flags, &portslot_lock);
		return 0;
	}
	rt_spin_unlock_irqrestore(flags, &portslot_lock);
	return -EINVAL;
}

static spinlock_t req_rel_lock = SPIN_LOCK_UNLOCKED;

static inline int hash_fun(unsigned long long owner)
{
	unsigned short *us;
	us = (unsigned short *)&owner;
	return ((us[0] >> 4) + us[3]) & MaxStubsMone;
}

static inline int hash_ins(unsigned long long owner)
{
	int i, k;
	unsigned long flags;

	i = hash_fun(owner);
	while (1) {
		k = i;
		while (portslot[k].owner) {
			if ((k = (k + 1) & MaxStubsMone) == i) {
				return 0;
			}
		}
		flags = rt_spin_lock_irqsave(&req_rel_lock);
		if (!portslot[k].owner) {
			break;
		}
		rt_spin_unlock_irqrestore(flags, &req_rel_lock);
	}
	portslot[k].owner = owner;
	rt_spin_unlock_irqrestore(flags, &req_rel_lock);
	return k;
}

static inline int hash_find(unsigned long long owner)
{
	int i, k;

	k = i = hash_fun(owner);
	while (portslot[k].owner != owner) {
		if (!portslot[k].owner || (k = (k + 1) & MaxStubsMone) == i) {
			return 0;
		}
	}
	return k;
}

static inline int hash_find_if_not_ins(unsigned long long owner)
{
	int i, k;
	unsigned long flags;

	i = hash_fun(owner);
	while (1) {
		k = i;
		while (portslot[k].owner && portslot[k].owner != owner) {
			if ((k = (k + 1) & MaxStubsMone) == i) {
				return 0;
			}
		}
		flags = rt_spin_lock_irqsave(&req_rel_lock);
		if (portslot[k].owner == owner) {
			rt_spin_unlock_irqrestore(flags, &req_rel_lock);
			return k;
		} else if (!portslot[k].owner) {
			break;
		}
		rt_spin_unlock_irqrestore(flags, &req_rel_lock);
	}
	portslot[k].owner = owner;
	rt_spin_unlock_irqrestore(flags, &req_rel_lock);
	return k;
}

static inline int hash_rem(unsigned long long owner)
{
	int i, k;
	unsigned long flags;

	i = hash_fun(owner);
	while (1) {
		k = i;
		while (portslot[k].owner != owner) {
			if (!portslot[k].owner || (k = (k + 1) & MaxStubsMone) == i) {
				return 0;
			}
		}
		flags = rt_spin_lock_irqsave(&req_rel_lock);
		if (portslot[k].owner == owner) {
			break;
		}
		rt_spin_unlock_irqrestore(flags, &req_rel_lock);
	}
	portslot[k].owner = 0;
	rt_spin_unlock_irqrestore(flags, &req_rel_lock);
	return k;
}

#define 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 + TIMER_FREQ/2 - 1)/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 { int op, port, priority, hard; unsigned long long owner; unsigned long name, chkspare;};

static void net_resume_task(int sock, SEM *sem)
{
	rt_sem_signal(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 *);

struct fun_args { int a[10]; long long (*fun)(int, ...); };

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

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;
}

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

static int soft_kthread_init(RT_TASK *task, int fun, int arg, int priority)
{
	task->magic = task->state = 0;
	(task->fun_args = (int *)(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 + TIMER_FREQ/2 - 1)/TIMER_FREQ);
		}
		return 0;
	}
	return -ENOEXEC;
}

static int soft_kthread_delete(RT_TASK *task)
{
	if (clr_rtext(task)) {
		return -EFAULT;
	} else {
		struct task_struct *lnxtsk = task->lnxtsk;
		lnxtsk->this_rt_task[0] = lnxtsk->this_rt_task[1] = 0;
		sigemptyset(&lnxtsk->blocked);
		lnxtsk->state = TASK_INTERRUPTIBLE;
		kill_proc(lnxtsk->pid, SIGTERM, 0);
	}
	return 0;
}

#define ADRSZ  sizeof(struct sockaddr)

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 { int priority, base_priority, argsize, rsize, fun_ext_timed; long long type; int a[1]; } *par;
	int *a, wsize, w2size, sock;
	long long type;

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

	while (soft_rt_fun_call(task, rt_sem_wait, sem) != SEM_ERR) {
		wsize = soft_rt_recvfrom(sock, msg, MAX_MSG_SIZE, 0, addr, &w2size);
		if (decode) {
			decode(portslotp, msg, wsize, RPC_SRV);
		}
		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;
		}
		type = par->type;
		if (par->rsize) {
			a[USP_RBF1(type) - 1] = (int)((char *)a + par->argsize);
		}
		if (NEED_TO_W(type)) {
			wsize = USP_WSZ1(type);
			wsize = wsize ? a[wsize - 1] : (USP_WSZ1LL(type) ? sizeof(long long) : sizeof(int));
		} else {
			wsize = 0;
		}
		if (NEED_TO_W2ND(type)) {
			w2size = USP_WSZ2(type);
			w2size = w2size ? a[w2size - 1] : (USP_WSZ2LL(type) ? sizeof(long long) : sizeof(int));
		} else {
			w2size = 0;
		}
		do {
			struct msg_t { int wsize, w2size; unsigned long long retval; char msg_buf[wsize], msg_buf2[w2size]; } arg;
			if (wsize > 0) {
				arg.wsize = wsize;
				a[USP_WBF1(type) - 1] = (int)arg.msg_buf;
			} else {
				arg.wsize = 0;
			}
			if (w2size > 0) {
				arg.w2size = w2size;
				a[USP_WBF2(type) - 1] = (int)arg.msg_buf2;
			} else {
				arg.w2size = 0;
			}
			if ((wsize = TIMED(par->fun_ext_timed) - 1) >= 0) {
				*((long long *)(a + wsize)) = nano2count(*((long long *)(a + wsize)));
			}
			arg.retval = soft_rt_genfun_call(task, rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun, a, par->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);
	}
	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 { int priority, base_priority, argsize, rsize, fun_ext_timed; long long type; int a[1]; } *par;
	int *a, wsize, w2size, sock;
	long long type;

	addr = (struct sockaddr *)&portslotp->addr;
	sock = portslotp->socket[1];
	sem  = &portslotp->sem;
	a = (par = (void *)msg)->a;
	task = (RT_TASK *)portslotp->task;
	sprintf(current->comm, "HRDSTB-%d", sock);

	while (rt_sem_wait(sem) != SEM_ERR) {
		wsize = hard_rt_recvfrom(sock, msg, MAX_MSG_SIZE, 0, addr, &w2size);
		if (decode) {
			decode(portslotp, msg, wsize, RPC_SRV);
		}
		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;
		}
		type = par->type;
		if (par->rsize) {
			a[USP_RBF1(type) - 1] = (int)((char *)a + par->argsize);
		}
		if (NEED_TO_W(type)) {
			wsize = USP_WSZ1(type);
			wsize = wsize ? a[wsize - 1] : (USP_WSZ1LL(type) ? sizeof(long long) : sizeof(int));
		} else {