shadow.c

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

	{
	splhigh(s);

	__xn_get_user(current,syncflag,u_syncp);

	if (syncflag)
	    break;

	set_current_state(TASK_INTERRUPTIBLE);

	splexit(s);

	schedule();
	}

    splexit(s);

    return syncflag == 0x7fffffff ? 0 : syncflag;
}

void xnshadow_exit (void)

{
    __adeos_schedule_back_root(current);
    do_exit(0);
}

static void xnshadow_asr (xnsigmask_t sigs)

{
    if (sigs & XNTHREAD_SHADOW_SIGKILL)
	xnpod_delete_self(NULL);
}

/*! 
 * \fn xnthread_t *xnshadow_map(xnthread_t *thread,
                                const char *name,
                                int prio,
				    pid_t syncpid,
				    int *u_syncp,
				    unsigned magic,
				    xnmutex_t *imutex);
 * \brief Create a shadow thread context -- INTERNAL.
 *
 * @param thread The descriptor address of the new shadow thread to be
 * mapped to "current".
 *
 * @param name An ASCII string standing for the symbolic name of the
 * shadow thread.
 *
 * @param prio The base priority of the new thread. This value must
 * range from [minpri .. maxpri] (inclusive) as specified when calling
 * the xnpod_init() service from the skin initialization routine.
 *
 * @param syncpid If non-zero, this must be the pid of a Linux task to
 * wake up when the shadow has been initialized. In this case, u_syncp
 * must be valid to, and the new shadow thread is left in a dormant
 * state (XNDORMANT) after its creation, leading to the suspension of
 * "current" in the RTAI domain. Otherwise, the shadow thread is
 * immediately started and "current" exits from this service without
 * being suspended.
 *
 * @param u_syncp If non-zero, this must be a pointer to an integer
 * variable into the caller's address space in user-space which will
 * be used as a semaphore. This semaphore will be posted to wakeup the
 * task identified by pid before "current" is suspended in dormant
 * state by this service. The awaken Linux task is expected to invoke
 * a syscall hat ends up calling xnshadow_start() to finally start the
 * newly created shadow. Passing a null pointer here has the same
 * effect as passing a zero pid argument, and there will be no attempt
 * to wake up any task.
 *
 * @param magic A magic value aimed at identifying unambiguously the
 * skin to which the shadow belongs to. This value must be non-zero.
 *
 * @param imutex The address of an interface mutex currently held by
 * the caller which will be subject to a lock-breaking preemption
 * before the rescheduling takes place in the __xn_sys_sched
 * service. Passing NULL when no lock-breaking preemption is required
 * is valid. See xnpod_schedule() for more on lock-breaking preemption
 * points.
 *
 * Side-effect: This routine indirectly calls the rescheduling
 * procedure.
 *
 * Context: This routine must be called on behalf of the Linux
 * user-space task which is being shadowed.
 */

void xnshadow_map (xnthread_t *thread,
		   const char *name,
		   int prio,
		   pid_t syncpid,
		   int *u_syncp, /* user-space pointer */
		   unsigned magic,
		   xnmutex_t *imutex)
{
    int autostart = !(syncpid && u_syncp);
    spl_t s;

    current->cap_effective |= CAP_TO_MASK(CAP_IPC_LOCK)|CAP_TO_MASK(CAP_SYS_RAWIO)|CAP_TO_MASK(CAP_SYS_NICE);

    xnthread_init(thread,
		  name,
		  prio,
		  XNSHADOW
#ifdef CONFIG_RTAI_FPU_SUPPORT
		  |XNFPU
#endif /* CONFIG_RTAI_FPU_SUPPORT */
		  ,
		  0,
		  NULL,
		  magic);

    thread->asr = &xnshadow_asr;
    thread->asrmode = 0;
    thread->asrimask = 0;

    xnarch_init_shadow_tcb(xnthread_archtcb(thread),thread,name);

    splhigh(s);
    appendq(&nkpod->threadq,&thread->glink);
    xnpod_suspend_thread(thread,
			 autostart ? XNRELAX : (XNRELAX|XNDORMANT),
			 XN_INFINITE,
			 NULL,
			 NULL);
    splexit(s);

    xnshadow_ptd(current) = thread;

    set_linux_task_priority(current,prio);

    if (!autostart)
	/* Wake up the initiating Linux task. */
	xnshadow_sync_post(syncpid,u_syncp,0);
    
#if 1
    if (traceme)
	printk("__MAP__ %s from %s, prio=%d, pid=%d, domain=%s\n",
	       name ? name : "<anonymous>",
	       xnpod_current_sched()->runthread->name,
	       xnthread_base_priority(thread),
	       xnthread_archtcb(thread)->user_task->pid,
	       adp_current->name);
#endif

    /* If not autostarting, the shadow will be left suspended in
       dormant state. */
    xnshadow_harden(imutex);

    if (autostart)
	/* We are immediately joining the RTAI realm on behalf of the
	   current Linux task. */
	xnshadow_start(thread,0,NULL,NULL,1);
    else if (xnshadow_ptd(current) == NULL)
	    /* Woops, this shadow was unmapped while in dormant state
	       (i.e. before xnshadow_start() has been called on
	       it). Ask Linux to reap it. */
	xnshadow_exit();
}

void xnshadow_start (xnthread_t *thread,
		     u_long mode,
		     void (*u_entry)(void *cookie),
		     void *u_cookie,
		     int resched)
{
    spl_t s;

    splhigh(s);

    setbits(thread->status,(mode & (XNTHREAD_MODE_BITS|XNSUSP))|XNSTARTED);
    thread->imask = 0;
    thread->imode = (mode & XNTHREAD_MODE_BITS);
    thread->entry = u_entry;	/* user-space pointer -- do not deref. */
    thread->cookie = u_cookie;	/* ditto. */
    thread->stime = xnarch_get_cpu_time();

    if (testbits(thread->status,XNRRB))
	thread->rrcredit = thread->rrperiod;

    xntimer_init(&thread->atimer,&xnshadow_itimer_handler,thread);

    xnpod_resume_thread(thread,XNDORMANT);

#if 1
    if (traceme)
	printk("__START__ %s (status=0x%lx), prio=%d, pid=%d, domain=%s (sched? %d)\n",
	       thread->name,
	       thread->status,
	       xnthread_base_priority(thread),
	       xnthread_archtcb(thread)->user_task->pid,
	       adp_current->name,
	       resched);
#endif

    splexit(s);

    if (resched)
	/* Reschedule on behalf of the RTAI domain. */
	xnshadow_schedule();
}

void xnshadow_renice (xnthread_t *thread)

{
    struct task_struct *task = xnthread_archtcb(thread)->user_task;
    spl_t s;

    splhigh(s);

    set_linux_task_priority(task,thread->cprio);

    if (xnpod_root_p() &&
	testbits(thread->status,XNRELAX) &&
	xnpod_priocompare(thread->cprio,xnpod_current_root()->cprio) > 0)
	xnpod_renice_root(thread->cprio);

    splexit(s);
}

static int xnshadow_attach_skin (struct task_struct *curr,
				 unsigned magic,
				 u_long infarg)
{
    xnsysinfo_t info;
    int muxid;
    spl_t s;

    if (infarg)
	{
	info.cpufreq = xnarch_get_cpu_freq();
	info.tickval = xnpod_get_tickval();
	__xn_copy_to_user(curr,(void *)infarg,&info,sizeof(info));
	}

    splhigh(s);

    for (muxid = 0; muxid < XENOMAI_MUX_NR; muxid++)
	{
	if (muxtable[muxid].magic == magic)
	    {
	    muxtable[muxid].refcnt++;
	    splexit(s);
	    return ++muxid;
	    }
	}

    splexit(s);

    return -ENOENT;
}

static int xnshadow_detach_skin (struct task_struct *curr, int muxid)

{
    xnholder_t *holder, *nholder;
    xnthread_t *thread;
    spl_t s;

    if (--muxid < 0 || muxid >= XENOMAI_MUX_NR)
	return -EINVAL;

    splhigh(s);

    muxtable[muxid].refcnt--;

    /* Find all active shadow threads belonging to the detached skin
       and delete them. Sidenote: there can only be one active primary
       interface (i.e. skin) declaring a real-time pod at a time, but
       additionally, there might be native nucleus threads
       (e.g. debugger) and/or threads belonging to secondary/helper
       interfaces which do not declare any pod, so we need to check
       their magic before attempting to delete them. */

    nholder = getheadq(&nkpod->threadq);

    while ((holder = nholder) != NULL)
	{
	nholder = nextq(&nkpod->threadq,holder);
	thread = link2thread(holder,glink);

	if (xnthread_magic(thread) == muxtable[muxid].magic &&
	    testbits(thread->status,XNSHADOW))
	    xnpod_delete_thread(thread,NULL);
	}

    splexit(s);

    return 0;
}

static int xnshadow_substitute_syscall (struct task_struct *curr,
					struct pt_regs *regs,
					int migrate)
{
    xnthread_t *thread = xnshadow_thread(curr);

    switch (__xn_reg_mux(regs))
	{
	case __NR_nanosleep:
	    
	    {
	    xnticks_t now, expire, delay;
	    struct timespec t;

	    if (!__xn_access_ok(curr,VERIFY_READ,(void *)__xn_reg_arg1(regs),sizeof(t)))
		{
		__xn_reg_rval(regs) = -EFAULT;
		return 1;
		}

	    __xn_copy_from_user(curr,&t,(void *)__xn_reg_arg1(regs),sizeof(t));

	    if (t.tv_nsec >= 1000000000L || t.tv_nsec < 0 || t.tv_sec < 0)
		{
		__xn_reg_rval(regs) = -EINVAL;
		return 1;
		}

	    if (migrate) /* Shall we migrate to RTAI first? */
		xnshadow_harden(NULL);

	    if (testbits(nkpod->status,XNTMPER))
		expire = nkpod->jiffies;
	    else
		expire = xnpod_get_cpu_time();

	    delay = xnshadow_ts2ticks(&t);
	    expire += delay;

	    if (delay > 0)
		xnpod_delay(delay);

	    if (testbits(nkpod->status,XNTMPER))
		now = nkpod->jiffies;
	    else
		now = xnpod_get_cpu_time();

	    if (now >= expire)
		__xn_reg_rval(regs) = 0;
	    else
		{
		if (__xn_reg_arg2(regs))
		    {
		    if (!__xn_access_ok(curr,VERIFY_WRITE,(void *)__xn_reg_arg2(regs),sizeof(t)))
			{
			__xn_reg_rval(regs) = -EFAULT;
			return 1;
			}

		    xnshadow_ticks2ts(now - expire,&t);
		    __xn_copy_to_user(curr,(void *)__xn_reg_arg2(regs),&t,sizeof(t));
		    }

		__xn_reg_rval(regs) = -1;
		}

	    return 1;
	    }

	case __NR_setitimer:

	    {
	    xnticks_t delay, interval, expire;
	    struct itimerval itv;

	    if (__xn_reg_arg1(regs) != ITIMER_REAL)
		return 0;

	    if (__xn_reg_arg2(regs))
		{
		if (!__xn_access_ok(curr,VERIFY_READ,(void *)__xn_reg_arg2(regs),sizeof(itv)))
		    {
		    __xn_reg_rval(regs) = -EFAULT;
		    return 1;
		    }

		__xn_copy_from_user(curr,&itv,(void *)__xn_reg_arg2(regs),sizeof(itv));
		}
	    else
		memset(&itv,0,sizeof(itv));

	    xntimer_stop(&thread->atimer);

	    delay = xnshadow_tv2ticks(&itv.it_value);
	    interval = xnshadow_tv2ticks(&itv.it_interval);

	    if (testbits(nkpod->status,XNTMPER))
		expire = nkpod->jiffies;
	    else
		expire = xnpod_get_cpu_time();

	    expire += delay;

	    if (delay > 0)
		xntimer_start(&thread->atimer,delay,interval);

	    if (__xn_reg_arg3(regs))
		{
		if (!__xn_access_ok(curr,VERIFY_WRITE,(void *)__xn_reg_arg3(regs),sizeof(itv)))
		    {
		    __xn_reg_rval(regs) = -EFAULT;
		    return 1;
		    }

		interval = xntimer_interval(&thread->atimer);

		if (xntimer_active_p(&thread->atimer))
		    {
		    delay = xntimer_get_timeout(&thread->atimer);

		    if (delay == 0)
			delay = 1;
		    }
		else
		    delay = 0;

		xnshadow_ticks2tv(delay,&itv.it_value);
		xnshadow_ticks2tv(interval,&itv.it_interval);
		__xn_copy_to_user(curr,(void *)__xn_reg_arg3(regs),&itv,sizeof(itv));
		}

	    __xn_reg_rval(regs) = 0;

	    return 1;
	    }

	case __NR_getitimer:

	    {
	    xnticks_t delay, interval;
	    struct itimerval itv;

	    if (__xn_reg_arg1(regs) != ITIMER_REAL)
		return 0;

	    if (!__xn_reg_arg2(regs) ||
		!__xn_access_ok(curr,VERIFY_WRITE,(void *)__xn_reg_arg2(regs),sizeof(itv)))
		{
		__xn_reg_rval(regs) = -EFAULT;
		return 1;
		}

	    interval = xntimer_interval(&thread->atimer);

	    if (xntimer_active_p(&thread->atimer))
		{
		delay = xntimer_get_timeout(&thread->atimer);

		if (delay == 0) /* Cannot be negative in this context. */
		    delay = 1;
		}
	    else
		delay = 0;

	    xnshadow_ticks2tv(delay,&itv.it_value);
	    xnshadow_ticks2tv(interval,&itv.it_interval);
	    __xn_copy_to_user(curr,(void *)__xn_reg_arg3(regs),&itv,sizeof(itv));
	    __xn_reg_rval(regs) = 0;

	    return 1;
	    }

	default:

	    /* No real-time replacement -- let Linux handle this call. */
	    return 0;
	}
}

static void xnshadow_realtime_sysentry (adevinfo_t *evinfo)

{
    struct pt_regs *regs = (struct pt_regs *)evinfo->evdata;
    struct task_struct *task;
    xnthread_t *thread;
    int muxid, muxop;

    if (!nkpod)
	{
	if (__xn_reg_mux_p(regs))
	    {
	    if (__xn_reg_mux(regs) != __xn_mux_code(0,__xn_sys_attach))
		printk("Xenomai: bad syscall %ld/%ld -- no skin loaded\n",
		       __xn_mux_id(regs),
		       __xn_mux_op(regs));

	    __xn_reg_rval(regs) = -ENOSYS;
	    }
	else
	    /* Regular Linux syscall with no skin loaded -- propagate
	       it to the Linux kernel. */
	    adeos_propagate_event(evinfo);

	return;
	}

    task = get_calling_task(evinfo);
    thread = xnshadow_thread(task);

    if (!__xn_reg_mux_p(regs))
	{
	if (xnpod_root_p())
	    {
	    /* The call originates from the Linux domain, just
	       propagate the event so that we will fall back to
	       xnshadow_linux_sysentry(). */
	    adeos_propagate_event(evinfo);
	    return;
	    }
#if 1
	if (traceme)
	    printk("__SHADOW__ %s (sched=%s, linux=%s), call=%ld, pid=%d, ilock=%ld, task %p, origdomain 0x%x\n",
		   xnpod_current_thread()->name,
		   thread->name,
		   task->comm,
		   __xn_reg_mux(regs),
		   task->pid,
		   adeos_test_pipeline_from(&irq_shield),
		   task,
		   evinfo->domid);
#endif

	if (!testbits(thread->status,XNRELAX) &&
	    xnshadow_substitute_syscall(task,regs,0))
	    /* This is a Linux syscall issued on behalf of a shadow
	       thread running inside the RTAI domain. This call has
	       just been intercepted by Xenomai and a RTAI replacement
	       has been substituted for it. */
	    return;

	/* This syscall has not been substituted, let Linux handle
	   it. This will eventually fall back to the Linux syscall
	   handler if our Linux domain handler does not intercept
	   it. Before we let it go, ensure that our running thread has
	   properly entered the Linux domain. */

	adeos_propagate_event(evinfo);

#if 1
	if (traceme)
	    printk("__SYSIN__ %s, call=%ld, pid=%d\n",
		   xnpod_current_thread()->name,
		   __xn_reg_mux(regs),
		   task->pid);
#endif

	if (!testbits(thread->status,XNRELAX))
	    {
	    if (testbits(thread->status,XNAUTOSW))
		setbits(thread->status,XNSYSSW);

	    xnshadow_relax();
	    }

	return;
	}

    muxid = __xn_mux_id(regs);
    muxop = __xn_mux_op(regs);

#if 1
    if (traceme)
	printk("REQ {skin=%d, op=%d} on behalf of thread %s, pid=%d in domain %s\n",
	       muxid,
	       muxop,
	       xnpod_current_thread()->name,
	       task->pid,
	       adp_current->name);
#endif

    if (muxid == 0)
	{
	/* Internal nucleus call for shadow support. */

	switch (muxop)
	    {