netrpc.c

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

			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 = ((long long (*)(int, ...))rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun)(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9]);
			hard_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
		} while (0);
	}
	rt_task_suspend(task);
}

static void trashmsg(struct portslot_t *portslotp, int hard)
{
	char msg[MAX_MSG_SIZE];
	if (hard) {
		hard_rt_recvfrom(portslotp->socket[hard], msg, MAX_MSG_SIZE, MSG_DONTWAIT, (void *)msg, (void *)msg);
	} else {
		soft_rt_recvfrom(portslotp->socket[hard], msg, MAX_MSG_SIZE, MSG_DONTWAIT, (void *)msg, (void *)msg);
	}
}

static void port_server_fun(RT_TASK *port_server)
{
	int i, rsize;
	RT_TASK *task;
	unsigned long flags;
	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) != SEM_ERR) {
	if ((rsize = hard_rt_recvfrom(portslot[0].socket[1], &msg, sizeof(msg), MSG_DONTWAIT, addr, &i)) <= 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) {
		i = msg.op - BASEPORT;
		if (i > 0 && i < MaxStubs) {
        		flags = rt_spin_lock_irqsave(&req_rel_lock);
			if (portslot[i].owner == msg.owner) {
				task = (RT_TASK *)portslot[i].task;
				portslot[i].task = 0;
				portslot[i].owner = 0;
				msg.port = msg.op;
       				rt_spin_unlock_irqrestore(flags, &req_rel_lock);
				if (task->is_hard) {
					rt_task_delete(task);
				} else {
					soft_kthread_delete(task);
				}
				kfree(task);
			} else {
				msg.port = !portslot[i].owner ? msg.op : -ENXIO;
       				rt_spin_unlock_irqrestore(flags, &req_rel_lock);
			}
		} else {
			msg.port = -EINVAL;
		}
		goto ret;
	}
	if ((msg.port = hash_find_if_not_ins(msg.owner)) <= 0) {
		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, (int)(portslot + msg.port), StackSize + 2*MAX_MSG_SIZE, msg.priority, 0, 0) : soft_kthread_init(task, (int)soft_stub_fun, (int)(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;
		}
		trashmsg(portslot + msg.port, msg.hard);
		portslot[msg.port].name = msg.name;
		portslot[msg.port].task = (unsigned long)(task);
		portslot[msg.port].sem.count = 0;
		portslot[msg.port].sem.queue.prev = portslot[msg.port].sem.queue.next = &portslot[msg.port].sem.queue;
		rt_task_resume(task);
	}
	msg.port += BASEPORT;
ret:
	if (msg.hard) {
		hard_rt_sendto(portslot[0].socket[1], &msg, encode ? encode(&portslot[0], &msg, sizeof(msg), PRT_RTR) : sizeof(msg), 0, addr, ADRSZ);
	} 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;

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

	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;
	} 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[msg.hard], id);
	msg.priority = task->base_priority;
	trashmsg(portslot + msg.port, msg.hard);
	msgsize = encode ? encode(&portslot[0], &msg, sizeof(msg), PRT_REQ) : sizeof(msg);
	for (i = 0; i < TIMER_FREQ && !portslotp->sem.count; i++) {
		if (msg.hard) {
			hard_rt_sendto(portslotp->socket[1], &msg, msgsize, 0, (void *)&portslotp->addr, ADRSZ);
		} else {
			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 = msg.hard ? hard_rt_recvfrom(portslotp->socket[1], &msg, sizeof(msg), 0, (void *)&portslotp->addr, &i) : 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 {
				trashmsg(portslot + msg.port, msg.hard);
				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->task = 1;
				return portslotp->indx;
			}
		}
	}
	gvb_portslot(portslotp);
	return msg.port ? msg.port : -ETIMEDOUT;
}

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

int rt_rel_stub(unsigned long long owner)
{
	return hash_rem(owner) > 0 ? 0 : -ESRCH;
}

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

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

unsigned long long rt_net_rpc(int fun_ext_timed, long long type, void *args, int argsize, int space)
{
	char msg[MAX_MSG_SIZE];
	struct reply_t { int wsize, w2size; unsigned long long retval; char msg[1]; } *reply;
	int rsize, port;
	struct portslot_t *portslotp;

	if ((port = PORT(fun_ext_timed)) > 0) {
		if ((portslotp = portslot + port)->task < 0) {
			int i;
			struct sockaddr addr;
			rt_sem_wait(&portslotp->sem);
			if ((rsize = portslotp->hard ? hard_rt_recvfrom(portslotp->socket[1], msg, MAX_MSG_SIZE, 0, &addr, &i) : soft_rt_recvfrom(portslotp->socket[0], msg, MAX_MSG_SIZE, 0, &addr, &i))) {
				if (decode) {
					rsize = decode(portslotp, msg, rsize, RPC_RCV);
				}
				mbx_send_if(portslotp->mbx, msg, rsize); 
			}
			portslotp->task = 1;
		}
		portslotp->msg = msg;
	} else {
		if ((portslotp = portslot - port)->task < 0) {
			if (!rt_sem_wait_if(&portslotp->sem)) {
				return 0;
			} else {
				int i;
				struct sockaddr addr;
				if ((rsize = portslotp->hard ? hard_rt_recvfrom(portslotp->socket[1], msg, MAX_MSG_SIZE, 0, &addr, &i) : soft_rt_recvfrom(portslotp->socket[0], msg, MAX_MSG_SIZE, 0, &addr, &i))) {
					if (decode) {
						rsize = decode(portslotp, msg, rsize, RPC_RCV);
					}
					mbx_send_if(portslotp->mbx, msg, rsize);
				}
			}
		} else {
			portslotp->task = -1;
		}
	}
	if (FUN(fun_ext_timed) == SYNC_NET_RPC) {
		return 1;
	}
	if (NEED_TO_R(type)) {			
		rsize = USP_RSZ1(type);
		rsize = rsize ? ((int *)args)[rsize - 1] : (USP_RSZ1LL(type) ? sizeof(long long) : sizeof(int));
	} else {
		rsize = 0;
	}
	do {
		struct msg_t { int priority, base_priority, argsize, rsize, fun_ext_timed; long long type; int args[1]; } *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;
		memcpy(arg->args, args, argsize);
		if (rsize > 0) {			
			if (space) {
				memcpy((char *)arg->args + argsize, (int *)((int *)args + USP_RBF1(type) - 1)[0], rsize);
			} else {
				copy_from_user((char *)arg->args + argsize, (int *)((int *)args + USP_RBF1(type) - 1)[0], rsize);
			}
		}
		rsize = sizeof(struct msg_t) - sizeof(int) + argsize + rsize;
		if (encode) {
			rsize = encode(portslotp, msg, rsize, RPC_REQ);
		}
		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;
		rt_sem_wait(&portslotp->sem);
		rsize = portslotp->hard ? hard_rt_recvfrom(portslotp->socket[1], msg, MAX_MSG_SIZE, 0, &addr, &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)->wsize) {
			if (space) {
				memcpy((char *)(*((int *)args + USP_WBF1(type) - 1)), reply->msg, reply->wsize);
			} else {
				copy_to_user((char *)(*((int *)args + USP_WBF1(type) - 1)), reply->msg, reply->wsize);
			}
		}
		if (reply->w2size) {
			if (space) {
				memcpy((char *)(*((int *)args + USP_WBF2(type) - 1)), reply->msg + reply->wsize, reply->w2size);
			} else {
				copy_to_user((char *)(*((int *)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 { int wsize, w2size; unsigned long long retval; } reply;
	int ret;

	if ((ret = ((int (*)(MBX *, ...))rt_net_rpc_fun_ext[NET_RPC_EXT][type].fun)(mbx, &reply, sizeof(reply), 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;
}

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

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

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

struct rt_native_fun_entry rt_netrpc_entries[] = {
        { { 1LL, rt_net_rpc           },	NETRPC },
	{ { 1LL, rt_send_req_rel_port },	SEND_REQ_REL_PORT },
	{ { 0LL, ddn2nl               },	DDN2NL },
	{ { 0LL, rt_set_this_node     },	SET_THIS_NODE },
	{ { 0LL, rt_find_asgn_stub    },	FIND_ASGN_STUB },
	{ { 0LL, rt_rel_stub          },	REL_STUB },
	{ { 0LL, rt_waiting_return    },	WAITING_RETURN },
	{ { 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 + TIMER_FREQ/2 - 1)/TIMER_FREQ);
}

#ifdef CONFIG_RTAI_NETRPC_RTNET

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)