netrpc.c

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

{
	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, int *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>

static int errno;

static _syscall3(int, poll, struct pollfd *, ufds, unsigned int, nfds, int, timeout)

static inline int kpoll(struct pollfd *ufds, unsigned int nfds, int timeout)
{
	int retval;
	mm_segment_t svdfs = get_fs();
	set_fs(KERNEL_DS);
	retval = poll(ufds, nfds, timeout);
	set_fs(svdfs);
	return retval;
}

static _syscall2(int, socketcall, int, call, void *, args)

static inline int ksocketcall(int call, void *args)
{
	int retval;
	mm_segment_t svdfs = get_fs();
	set_fs(KERNEL_DS);
	retval = socketcall(call, args);
	set_fs(svdfs);
	return retval;
}

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[2])
{
	struct { int family; int type; int protocol; int usockvec[2]; } args = { family, type, protocol, { usockvec[1], usockvec[2] } };
	return ksocketcall(SYS_SOCKETPAIR, &args);
}
 
static inline int ksend(int fd, void *buff, size_t len, unsigned flags)
{
	struct { int fd; void *buff; size_t len; unsigned flags; } args = { fd, buff, len, flags };
	return ksocketcall(SYS_SEND, &args);
}

static inline int krecv(int fd, void *ubuf, size_t len, unsigned flags)
{
	struct { int fd; void *ubuf; size_t len; unsigned flags; } args = { fd, ubuf, len, flags };
	return ksocketcall(SYS_RECV, &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);
}

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) {
		down(&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, -1)) > 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);
}

#else

int init_softrtnet(void)
{
	return 0;
}

void cleanup_softrtnet(void)
{
	return;
}

#endif /* !CONFIG_RTAI_NETRPC_RTNET */

/*
 * 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;

	for (i = 8*sizeof(unsigned long) - 1; !test_bit(i, &MaxStubs); i--);
	if ((1 << i) != MaxStubs) {
		printk("MAX_STUBS (%lu): must be a power of 2.\n", MaxStubs);
		MaxStubs = 1 << (i + 1);
		printk("MAX_STUBS (%lu): forced to a power of 2.\n", MaxStubs);
	}
	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;
	}
	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 = 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].sem);
		hard_rt_socket_callback(portslot[i].socket[1], (void *)net_resume_task, &portslot[i].sem);
		portslot[i].owner = 0;
		rt_typed_sem_init(&portslot[i].sem, 0, BIN_SEM | FIFO_Q);
		portslot[i].task = 0;
	}
	SPRT_ADDR.sin_port = htons(BASEPORT);
	portslotsp = MaxStubs;
	portslot[0].name = PRTSRVNAME;
	portslot[0].owner = OWNER(this_node, (unsigned long)port_server);
	port_server = kmalloc(sizeof(RT_TASK) + 3*sizeof(struct fun_args), GFP_KERNEL);
	soft_kthread_init(port_server, (int)port_server_fun, (int)port_server, RT_SCHED_LOWEST_PRIORITY);
	portslot[0].task = (int)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);
	soft_kthread_delete(port_server);
	kfree(port_server);
	rt_sem_delete(&timer_sem);
	for (i = 0; i < MaxStubs; i++) {
		if (portslot[i].task) {
			rt_task_delete((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 */

#ifdef CONFIG_KBUILD
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);

#ifdef CONFIG_RTAI_NETRPC_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 /* CONFIG_RTAI_NETRPC_RTNET */

EXPORT_SYMBOL(rt_net_rpc_fun_hook);
#endif /* CONFIG_KBUILD */