shadow.c

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

/*!\file shadow.c
 * \brief Real-time shadow services.
 * \author Philippe Gerum
 *
 * Copyright (C) 2001,2002,2003 Philippe Gerum <[e-mail]>.
 *
 * Xenomai 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.
 *
 * Xenomai 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 Xenomai; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * As a special exception, the RTAI project gives permission
 * for additional uses of the text contained in its release of
 * Xenomai.
 *
 * The exception is that, if you link the Xenomai libraries with other
 * files to produce an executable, this does not by itself cause the
 * resulting executable to be covered by the GNU General Public License.
 * Your use of that executable is in no way restricted on account of
 * linking the Xenomai libraries code into it.
 *
 * This exception does not however invalidate any other reasons why
 * the executable file might be covered by the GNU General Public
 * License.
 *
 * This exception applies only to the code released by the
 * RTAI project under the name Xenomai.  If you copy code from other
 * RTAI project releases into a copy of Xenomai, as the General Public
 * License permits, the exception does not apply to the code that you
 * add in this way.  To avoid misleading anyone as to the status of
 * such modified files, you must delete this exception notice from
 * them.
 *
 * If you write modifications of your own for Xenomai, it is your
 * choice whether to permit this exception to apply to your
 * modifications. If you do not wish that, delete this exception
 * notice.
 *
 * \ingroup shadow
 */

/*!
 * \ingroup xenomai
 * \defgroup shadow Real-time shadow services.
 *
 * Real-time shadow services.
 *
 *@{*/

#define XENO_SHADOW_MODULE
#include <asm/signal.h>
#include <linux/unistd.h>
#include "rtai_config.h"
#include "xenomai/pod.h"
#include "xenomai/heap.h"
#include "xenomai/mutex.h"
#include "xenomai/synch.h"
#include "xenomai/module.h"
#include "xenomai/shadow.h"
#if CONFIG_TRACE
#include <linux/trace.h>
#endif

#define XENOMAI_MUX_NR 16

int nkgkptd;

static int traceme = 0;

static adomain_t irq_shield;

static struct task_struct *gatekeeper;

static struct semaphore gksync;

static struct semaphore gkreq;

static int gkstop;

static unsigned gkvirq;

static unsigned sigvirq;

static unsigned nicevirq;

static int gk_enter_in,
           gk_enter_out;

static struct {

    xnthread_t *thread;
    xnmutex_t *mutex;

} gk_enter_wheel[XNSHADOW_MAXRQ];

static int gk_leave_in,
           gk_leave_out;

static struct task_struct *gk_leave_wheel[XNSHADOW_MAXRQ];

static int gk_signal_in,
           gk_signal_out;

static struct {

    struct task_struct *task;
    int sig;

} gk_signal_wheel[XNSHADOW_MAXRQ];

static int gk_renice_in,
           gk_renice_out;

static struct {

    struct task_struct *task;
    int prio;

} gk_renice_wheel[XNSHADOW_MAXRQ];

static struct {

    unsigned magic;
    int nrcalls;
    int refcnt;
    xnsysent_t *systab;

} muxtable[XENOMAI_MUX_NR];

static inline struct task_struct *get_calling_task (adevinfo_t *evinfo) {

    return evinfo->domid == adp_current->domid ? current : rtai_get_root_current(0);
}

static inline void set_linux_task_priority (struct task_struct *task, int prio)

{
    spl_t s;

    /* Can be called on behalf of RTAI -- The priority of the
       real-time shadow will be used to determine the Linux
       task priority. */

    if (adp_current == adp_root)
	{
	rtai_set_linux_task_priority(task,SCHED_FIFO,prio);
	return;
	}

    splhigh(s);
    gk_renice_wheel[gk_renice_in].task = task;
    gk_renice_wheel[gk_renice_in].prio = prio;
    gk_renice_in = (gk_renice_in + 1) & (XNSHADOW_MAXRQ - 1);
    splexit(s);

    adeos_propagate_irq(nicevirq);
}

static void engage_irq_shield (void)

{
    adeos_declare_cpuid;
    unsigned long flags;
    unsigned irq;

    /* Since the interrupt shield does not handle the virtual IRQs we
       use for sending commands to Linux (e.g. wakeups, renice
       requests etc.), those will be marked pending in Linux's IRQ log
       when scheduled and get played normally, without any explicit
       propagation needed from the shielding domain, even if the
       latter is stalled. */

    adeos_stall_pipeline_from(&irq_shield);

    adeos_lock_cpu(flags);

    for (irq = 0; irq < IPIPE_NR_XIRQS; irq++)
	__adeos_lock_irq(adp_root,cpuid,irq);

    adeos_unlock_cpu(flags);
}

static void disengage_irq_shield (void)

{
    unsigned long flags;
    unsigned irq;

    rtai_hw_lock(flags);

    for (irq = 0; irq < IPIPE_NR_XIRQS; irq++)
	__adeos_unlock_irq(adp_root,irq);

    rtai_hw_unlock(flags);

    adeos_unstall_pipeline_from(&irq_shield);
}

static void xnshadow_renice_handler (unsigned virq)

{
    splnone();	/* Unstall the root stage first since the renice
		   operation might sleep.  */
    while (gk_renice_out != gk_renice_in)
	{
	struct task_struct *task = gk_renice_wheel[gk_renice_out].task;
	int prio = gk_renice_wheel[gk_renice_out].prio;
	gk_renice_out = (gk_renice_out + 1) & (XNSHADOW_MAXRQ - 1);
	rtai_set_linux_task_priority(task,SCHED_FIFO,prio);
	}
}

static void xnshadow_wakeup_handler (unsigned virq)

{
    int shield_on = 0;

    while (gk_leave_out != gk_leave_in)
	{
	struct task_struct *task = gk_leave_wheel[gk_leave_out];
	gk_leave_out = (gk_leave_out + 1) & (XNSHADOW_MAXRQ - 1);

	if (xnshadow_thread(task) && !shield_on)
	    {
	    engage_irq_shield();
	    shield_on = 1;
	    }

	wake_up_process(task);
	}
}

static void xnshadow_signal_handler (unsigned virq)

{
    while (gk_signal_out != gk_signal_in)
	{
	struct task_struct *task = gk_signal_wheel[gk_signal_out].task;
	int sig = gk_signal_wheel[gk_signal_out].sig;
	gk_signal_out = (gk_signal_out + 1) & (XNSHADOW_MAXRQ - 1);
	send_sig(sig,task,1);
	}
}

static inline void xnshadow_sched_wakeup (struct task_struct *task)

{
    spl_t s;

    splhigh(s);
    gk_leave_wheel[gk_leave_in] = task;
    gk_leave_in = (gk_leave_in + 1) & (XNSHADOW_MAXRQ - 1);
    splexit(s);

    /* Do _not_ use adeos_propagate_irq() here since we might need to
       schedule a wakeup on behalf of the Linux domain. */

    adeos_schedule_irq(gkvirq);

#if CONFIG_TRACE
    TRACE_PROCESS(TRACE_EV_PROCESS_SIGNAL, -111, task->pid);
#endif
}

static inline void xnshadow_sched_signal (struct task_struct *task, int sig)

{
    spl_t s;

    splhigh(s);
    gk_signal_wheel[gk_signal_in].task = task;
    gk_signal_wheel[gk_signal_in].sig = sig;
    gk_signal_in = (gk_signal_in + 1) & (XNSHADOW_MAXRQ - 1);
    splexit(s);

    adeos_propagate_irq(sigvirq);
}

static void xnshadow_itimer_handler (void *cookie)

{
    xnthread_t *thread = (xnthread_t *)cookie;
    xnshadow_sched_signal(xnthread_archtcb(thread)->user_task,SIGALRM);
}

static void gatekeeper_thread (void)

{
    atomic_counter_t imutexval;

    gatekeeper = current;

    self_daemonize("gatekeeper");

    sigfillset(&current->blocked);

    up(&gksync);

    for (;;)
	{
	set_linux_task_priority(current,1);

	down_interruptible(&gkreq);

	if (gkstop)
	    break;

	while (gk_enter_out != gk_enter_in)
	    {
	    xnthread_t *thread = gk_enter_wheel[gk_enter_out].thread;
	    xnmutex_t *imutex = gk_enter_wheel[gk_enter_out].mutex;
	    gk_enter_out = (gk_enter_out + 1) & (XNSHADOW_MAXRQ - 1);
#if 1
	    if (traceme)
		printk("__GK__ %s (ipipe=%lu)\n",
		       thread->name,
		       adeos_test_pipeline_from(&rtai_domain));
#endif
	    xnpod_resume_thread(thread,XNRELAX);

	    if (imutex)
		xnmutex_clear_lock(imutex,&imutexval);
	    }

	xnpod_renice_root(XNPOD_ROOT_PRIO_BASE);

	/* Reschedule on behalf of the RTAI domain reflecting all
	   changes in a row. */
	xnshadow_schedule();
	}

    up(&gksync);
}

/* timespec/timeval <-> ticks conversion routines -- Lifted and
  adapted from include/linux/time.h. */

unsigned long long xnshadow_ts2ticks (const struct timespec *v)

{
    u_long hz = (u_long)xnpod_get_ticks2sec();
    unsigned long long nsec = v->tv_nsec;
    u_long sec = v->tv_sec;

    nsec += xnarch_ulldiv(1000000000LL,hz,NULL) - 1;
    nsec = xnarch_ulldiv(nsec,1000000000L / hz,NULL);

    return hz * sec + nsec;
}

void xnshadow_ticks2ts (unsigned long long ticks, struct timespec *v)

{
    u_long hz = (u_long)xnpod_get_ticks2sec(), mod;
    v->tv_nsec = xnarch_ullmod(ticks,hz,&mod) * (1000000000L / hz);
    v->tv_sec = xnarch_ulldiv(ticks,hz,NULL);
}

unsigned long long xnshadow_tv2ticks (const struct timeval *v)

{
    unsigned long long nsec = v->tv_usec * 1000LL;
    u_long hz = (u_long)xnpod_get_ticks2sec();
    u_long sec = v->tv_sec;

    nsec += xnarch_ulldiv(1000000000LL,hz,NULL) - 1;
    nsec = xnarch_ulldiv(nsec,1000000000L / hz,NULL);

    return hz * sec + nsec;
}

void xnshadow_ticks2tv (unsigned long long ticks, struct timeval *v)

{
    u_long hz = (u_long)xnpod_get_ticks2sec(), mod;
    v->tv_usec = xnarch_ullmod(ticks,hz,&mod) * (1000000L / hz);
    v->tv_sec = xnarch_ulldiv(ticks,hz,NULL);
}

/*! 
 * \fn void xnshadow_harden(xnmutex_t *imutex);
 * \brief Migrate a Linux task to the RTAI domain -- INTERNAL.
 *
 * This service causes the transition of "current" from the Linux
 * domain to RTAI. This is obtained by asking the gatekeeper to resume
 * the shadow mated with "current" then triggering the rescheduling
 * procedure in the RTAI domain. The shadow will resume in the RTAI
 * domain as returning from schedule().
 *
 * @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. The ability to pass a mutex through this service is
 * indirectly used by skins when creating a new shadow thread (see
 * xnshadow_map()), thus preventing any deletion while the thread
 * descriptor is accessed by the internal bootstrap code. 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 triggers the rescheduling
 * procedure (see __xn_sys_sched service).
 *
 * Context: This routine must be called on behalf of a user-space task
 * from the Linux domain.
 */

void xnshadow_harden (xnmutex_t *imutex)

{
    spl_t s;

#if 1
    if (traceme)
	printk("_!HARDENING!_ %s, status 0x%lx, pid=%d (ipipe=%lu, domain=%s)\n",
	       xnshadow_thread(current)->name,
	       xnshadow_thread(current)->status,
	       current->pid,
	       adeos_test_pipeline_from(&rtai_domain),
	       adp_current->name);
#endif
    /* Enqueue the request to move "current" from the Linux domain to
       the RTAI domain. This will cause the shadow thread to resume
       using the register state of the Linux task. */

    splhigh(s);

    gk_enter_wheel[gk_enter_in].thread = xnshadow_thread(current);
    gk_enter_wheel[gk_enter_in].mutex = imutex;
    gk_enter_in = (gk_enter_in + 1) & (XNSHADOW_MAXRQ - 1);

    engage_irq_shield();

    splexit(s);

    if (xnshadow_thread(current)->cprio > gatekeeper->rt_priority)
	set_linux_task_priority(gatekeeper,xnshadow_thread(current)->cprio);

    set_current_state(TASK_INTERRUPTIBLE);

    /* Wake up the gatekeeper which will perform the transition. */
    up(&gkreq);

    schedule();

#ifdef CONFIG_RTAI_FPU_SUPPORT
    xnpod_switch_fpu();
#endif /* CONFIG_RTAI_FPU_SUPPORT */

    splnone();

    /* "current" is now running into the RTAI domain. */

#if 1
    if (traceme)
	printk("__RT__ %s, pid=%d (ipipe=%lu)\n",
	       xnpod_current_thread()->name,
	       current->pid,
	       adeos_test_pipeline_from(&rtai_domain));
#endif
}

/*! 
 * \fn void xnshadow_relax(void);
 * \brief Switch a shadow thread back to the Linux domain -- INTERNAL.
 *
 * This service yields the control of the running shadow back to
 * Linux. This is obtained by suspending the shadow and scheduling a
 * wake up call for the mated user task inside the Linux domain. The
 * Linux task will resume on return from xnpod_suspend_thread() on
 * behalf of the root thread.
 *
 * Side-effect: This routine indirectly calls the rescheduling
 * procedure.
 *
 * Context: This routine must be called on behalf of a real-time
 * shadow inside the RTAI domain.
 */

void xnshadow_relax (void)

{
    xnthread_t *thread = xnpod_current_thread();
    spl_t s;

    /* Enqueue the request to move the running shadow from the RTAI
       domain to the Linux domain.  This will cause the Linux task
       to resume using the register state of the shadow thread. */

#if 1
    if (traceme)
	printk("_!RELAXING!_ %s, status 0x%lx, pid=%d (ipipe=%lu, domain=%s)\n",
	       thread->name,
	       thread->status,
	       xnthread_archtcb(thread)->user_task->pid,
	       adeos_test_pipeline_from(&rtai_domain),
	       adp_current->name);
#endif

    xnshadow_sched_wakeup(xnthread_archtcb(thread)->user_task);
    xnpod_renice_root(thread->cprio);
    splhigh(s);
    xnpod_suspend_thread(thread,XNRELAX,XN_INFINITE,NULL,NULL);
    __adeos_schedule_back_root(current);
    splexit(s);

    /* "current" is now running into the Linux domain on behalf of the
       root thread. */
#if 1
    if (traceme)
	printk("__RELAX__ %s (on %s, status 0x%lx), pid=%d (ipipe=%lu, domain=%s)\n",
	       thread->name,
	       xnpod_current_sched()->runthread->name,
	       xnpod_current_sched()->runthread->status,
	       xnthread_archtcb(thread)->user_task->pid,
	       adeos_test_pipeline_from(&rtai_domain),
	       adp_current->name);
#endif
}

void xnshadow_unmap (xnthread_t *thread) /* Must be called by the task deletion hook. */

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

    if (!task)
	return;

#if 1
    if (traceme)
	printk("__UNMAP__: %s, pid=%d, task=%s (ipipe=%lu, domain=%s, taskstate=%ld)\n",
	       thread->name,
	       task->pid,
	       task->comm,
	       adeos_test_pipeline_from(&rtai_domain),
	       adp_current->name,
	       task->state);
#endif

    xnshadow_ptd(task) = NULL;

    /* The zombie side returning to user-space will be trapped and
       exited inside the pod's rescheduling routines. */
    xnshadow_sched_wakeup(task);
}

static void xnshadow_sync_post (pid_t syncpid, int *u_syncp, int err)

{
    struct task_struct *synctask;

    /* FIXME: this won't be SMP safe. */

    read_lock(&tasklist_lock);
    synctask = find_task_by_pid(syncpid);
    read_unlock(&tasklist_lock);

    if (synctask)
	{
	__xn_put_user(synctask,err ?: 0x7fffffff,u_syncp); /* Poor man's semaphore V. */
	wake_up_process(synctask);
	}
}

static int xnshadow_sync_wait (int *u_syncp)

{
    int s, syncflag;

    for (;;)	/* Poor man's semaphore P. */