base/sched/sched.c

Go to the documentation of this file.

/*
 * Copyright (C) 1999-2008 Paolo Mantegazza <mantegazza@aero.polimi.it>
 *
 * 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.
 */

/*
ACKNOWLEDGMENTS:
- Steve Papacharalambous (stevep@zentropix.com) has contributed a very 
  informative proc filesystem procedure.
- Stefano Picerno (stefanopp@libero.it) for suggesting a simple fix to
  distinguish a timeout from an abnormal retrun in timed sem waits.
- Geoffrey Martin (gmartin@altersys.com) for a fix to functions with timeouts.
*/


#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/slab.h>
#include <linux/timex.h>
#include <linux/sched.h>
#include <linux/irq.h>
#include <linux/reboot.h>
#include <linux/sys.h>
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,18)
#include <linux/oom.h>
#endif

#include <asm/param.h>
#include <asm/system.h>
#include <asm/io.h>
#include <asm/uaccess.h>
#include <asm/mmu_context.h>

#define __KERNEL_SYSCALLS__
#include <linux/unistd.h>

#ifdef CONFIG_PROC_FS
#include <linux/stat.h>
#include <linux/proc_fs.h>
#include <rtai_proc_fs.h>
static int rtai_proc_sched_register(void);
static void rtai_proc_sched_unregister(void);
int rtai_proc_lxrt_register(void);
void rtai_proc_lxrt_unregister(void);
#endif

#include <rtai.h>
#include <asm/rtai_sched.h>
#include <rtai_lxrt.h>
#include <rtai_registry.h>
#include <rtai_nam2num.h>
#include <rtai_schedcore.h>
#include <rtai_prinher.h>
#include <rtai_signal.h>

MODULE_LICENSE("GPL");

/* +++++++++++++++++ WHAT MUST BE AVAILABLE EVERYWHERE ++++++++++++++++++++++ */

RT_TASK rt_smp_linux_task[NR_RT_CPUS];

RT_TASK *rt_smp_current[NR_RT_CPUS];

RTIME rt_smp_time_h[NR_RT_CPUS];

int rt_smp_oneshot_timer[NR_RT_CPUS];

volatile int rt_sched_timed;

static struct klist_t wake_up_sth[NR_RT_CPUS];
struct klist_t wake_up_hts[NR_RT_CPUS];
struct klist_t wake_up_srq[NR_RT_CPUS];

/* +++++++++++++++ END OF WHAT MUST BE AVAILABLE EVERYWHERE +++++++++++++++++ */

extern struct { volatile int locked, rqsted; } rt_scheduling[];

static unsigned long rt_smp_linux_cr0[NR_RT_CPUS];

static RT_TASK *rt_smp_fpu_task[NR_RT_CPUS];

static int rt_smp_half_tick[NR_RT_CPUS];

static int rt_smp_oneshot_running[NR_RT_CPUS];

static volatile int rt_smp_timer_shot_fired[NR_RT_CPUS];

static struct rt_times *linux_times;

static RT_TASK *lxrt_wdog_task[NR_RT_CPUS];

RT_TASK *lxrt_prev_task[NR_RT_CPUS];

static int lxrt_notify_reboot(struct notifier_block *nb,
                  unsigned long event,
                  void *ptr);

static struct notifier_block lxrt_reboot_notifier = {
    .notifier_call  = &lxrt_notify_reboot,
    .next       = NULL,
    .priority   = 0
};

static struct klist_t klistm[NR_RT_CPUS];

static struct task_struct *kthreadm[NR_RT_CPUS];

static struct semaphore resem[NR_RT_CPUS];

static int endkthread;

#define fpu_task (rt_smp_fpu_task[cpuid])

#define rt_half_tick (rt_smp_half_tick[cpuid])

#define oneshot_running (rt_smp_oneshot_running[cpuid])

#define oneshot_timer_cpuid (rt_smp_oneshot_timer[rtai_cpuid()])

#define timer_shot_fired (rt_smp_timer_shot_fired[cpuid])

#define rt_times (rt_smp_times[cpuid])

#define linux_cr0 (rt_smp_linux_cr0[cpuid])

#define MAX_FRESTK_SRQ  (2 << 6)
static struct { int srq; volatile unsigned long in, out; void *mp[MAX_FRESTK_SRQ]; } frstk_srq;

#define KTHREAD_M_PRIO MAX_LINUX_RTPRIO
#define KTHREAD_F_PRIO MAX_LINUX_RTPRIO

#ifdef CONFIG_SMP

extern void rt_set_sched_ipi_gate(void);
extern void rt_reset_sched_ipi_gate(void);
static void rt_schedule_on_schedule_ipi(void);

static inline int rt_request_sched_ipi(void)
{
        int retval;
        retval = rt_request_irq(SCHED_IPI, (void *)rt_schedule_on_schedule_ipi, NULL, 0);
        rt_set_sched_ipi_gate();
        return retval;
}

#define rt_free_sched_ipi() \
do { \
        rt_release_irq(SCHED_IPI); \
        rt_reset_sched_ipi_gate(); \
} while (0)

static inline void sched_get_global_lock(int cpuid)
{
    barrier(); 
    if (!test_and_set_bit(cpuid, &rtai_cpu_lock[0])) { 
        rtai_spin_glock(&rtai_cpu_lock[0]);
    }
    barrier();
}

static inline void sched_release_global_lock(int cpuid)
{
    barrier();
    if (test_and_clear_bit(cpuid, &rtai_cpu_lock[0])) {
        rtai_spin_gunlock(&rtai_cpu_lock[0]);
    } 
    barrier(); 
}

#else /* !CONFIG_SMP */

#define rt_request_sched_ipi()  0

#define rt_free_sched_ipi()

#define sched_get_global_lock(cpuid)

#define sched_release_global_lock(cpuid)

#endif /* CONFIG_SMP */

/* ++++++++++++++++++++++++++++++++ TASKS ++++++++++++++++++++++++++++++++++ */

#ifdef CONFIG_RTAI_MALLOC
int rtai_kstack_heap_size = (CONFIG_RTAI_KSTACK_HEAPSZ*1024);
RTAI_MODULE_PARM(rtai_kstack_heap_size, int);

static rtheap_t rtai_kstack_heap;

#define rt_kstack_alloc(sz)  rtheap_alloc(&rtai_kstack_heap, sz, 0)
#define rt_kstack_free(p)    rtheap_free(&rtai_kstack_heap, p)
#else
#define rt_kstack_alloc(sz)  rt_malloc(sz)
#define rt_kstack_free(p)    rt_free(p)
#endif

static int tasks_per_cpu[NR_RT_CPUS] = { 0, };

int get_min_tasks_cpuid(void)
{
    int i, cpuid, min;
    min =  tasks_per_cpu[cpuid = 0];
    for (i = 1; i < num_online_cpus(); i++) {
        if (tasks_per_cpu[i] < min) {
            min = tasks_per_cpu[cpuid = i];
        }
    }
    return cpuid;
}

void put_current_on_cpu(int cpuid)
{
#ifdef CONFIG_SMP
    struct task_struct *task = current;
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
    task->cpus_allowed = 1 << cpuid;
    while (cpuid != rtai_cpuid()) {
        task->state = TASK_INTERRUPTIBLE;
        schedule_timeout(2);
    }
#else /* KERNEL_VERSION >= 2.6.0 */
    if (set_cpus_allowed(task, cpumask_of_cpu(cpuid))) {
        set_cpus_allowed(current, cpumask_of_cpu(((RT_TASK *)(task->rtai_tskext(TSKEXT0)))->runnable_on_cpus = rtai_cpuid()));
    }
#endif  /* KERNEL_VERSION < 2.6.0 */
#endif /* CONFIG_SMP */
}

int set_rtext(RT_TASK *task, int priority, int uses_fpu, void(*signal)(void), unsigned int cpuid, struct task_struct *relink)
{
    unsigned long flags;

    if (num_online_cpus() <= 1) {
         cpuid = 0;
    }
    if (task->magic == RT_TASK_MAGIC || cpuid >= NR_RT_CPUS || priority < 0) {
        return -EINVAL;
    } 
    if (lxrt_wdog_task[cpuid] &&
        lxrt_wdog_task[cpuid] != task &&
        priority == RT_SCHED_HIGHEST_PRIORITY) {
             rt_printk("Highest priority reserved for RTAI watchdog\n");
         return -EBUSY;
    }
    task->uses_fpu = uses_fpu ? 1 : 0;
    task->runnable_on_cpus = cpuid;
    (task->stack_bottom = (long *)&task->fpu_reg)[0] = 0;
    task->magic = RT_TASK_MAGIC; 
    task->policy = 0;
    task->owndres = 0;
    task->prio_passed_to = 0;
    task->period = 0;
    task->resume_time = RT_TIME_END;
    task->periodic_resume_time = RT_TIME_END;
    task->queue.prev = task->queue.next = &(task->queue);      
    task->queue.task = task;
    task->msg_queue.prev = task->msg_queue.next = &(task->msg_queue);      
    task->msg_queue.task = task;    
    task->msg = 0;  
    task->ret_queue.prev = task->ret_queue.next = &(task->ret_queue);
    task->ret_queue.task = NULL;
    task->tprev = task->tnext = task->rprev = task->rnext = task;
    task->blocked_on = NULL;        
    task->signal = signal;
    task->unblocked = 0;
    task->rt_signals = NULL;
    memset(task->task_trap_handler, 0, RTAI_NR_TRAPS*sizeof(void *));
    task->linux_syscall_server = NULL;
    task->busy_time_align = 0;
    task->resync_frame = 0;
    task->ExitHook = 0;
    task->usp_flags = task->usp_flags_mask = task->force_soft = 0;
    task->msg_buf[0] = 0;
    task->exectime[0] = task->exectime[1] = 0;
    task->system_data_ptr = 0;
    atomic_inc((atomic_t *)(tasks_per_cpu + cpuid));
    if (relink) {
        task->priority = task->base_priority = priority;
        task->suspdepth = task->is_hard = 1;
        task->state = RT_SCHED_READY | RT_SCHED_SUSPENDED;
        relink->rtai_tskext(TSKEXT0) = task;
        task->lnxtsk = relink;
    } else {
        task->priority = task->base_priority = BASE_SOFT_PRIORITY + priority;
        task->suspdepth = task->is_hard = 0;
        task->state = RT_SCHED_READY;
        current->rtai_tskext(TSKEXT0) = task;
        current->rtai_tskext(TSKEXT1) = task->lnxtsk = current;
        put_current_on_cpu(cpuid);
    }
    flags = rt_global_save_flags_and_cli();
    task->next = 0;
    rt_linux_task.prev->next = task;
    task->prev = rt_linux_task.prev;
    rt_linux_task.prev = task;
    rt_global_restore_flags(flags);

    task->resq.prev = task->resq.next = &task->resq;
    task->resq.task = NULL;

    return 0;
}


static void start_stop_kthread(RT_TASK *, void (*)(long), long, int, int, void(*)(void), int);

int rt_kthread_init_cpuid(RT_TASK *task, void (*rt_thread)(long), long data,
            int stack_size, int priority, int uses_fpu,
            void(*signal)(void), unsigned int cpuid)
{
    start_stop_kthread(task, rt_thread, data, priority, uses_fpu, signal, cpuid);
    return (int)task->retval;
}


int rt_kthread_init(RT_TASK *task, void (*rt_thread)(long), long data,
            int stack_size, int priority, int uses_fpu,
            void(*signal)(void))
{
    return rt_kthread_init_cpuid(task, rt_thread, data, stack_size, priority, 
                 uses_fpu, signal, get_min_tasks_cpuid());
}


#if USE_RTAI_TASKS

asmlinkage static void rt_startup(void(*rt_thread)(long), long data)
{
    extern int rt_task_delete(RT_TASK *);
    RT_TASK *rt_current = rt_smp_current[rtai_cpuid()];
    rt_global_sti();
#if CONFIG_RTAI_MONITOR_EXECTIME
    rt_current->exectime[1] = rdtsc();
#endif
    ((void (*)(long))rt_current->max_msg_size[0])(rt_current->max_msg_size[1]);
    rt_drg_on_adr(rt_current);
    rt_task_delete(rt_smp_current[rtai_cpuid()]);
    rt_printk("LXRT: task %p returned but could not be delated.\n", rt_current); 
}


int rt_task_init_cpuid(RT_TASK *task, void (*rt_thread)(long), long data, int stack_size, int priority, int uses_fpu, void(*signal)(void), unsigned int cpuid)
{
    long *st, i;
    unsigned long flags;

    if (num_online_cpus() <= 1) {
         cpuid = 0;
    }
    if (task->magic == RT_TASK_MAGIC || cpuid >= NR_RT_CPUS || priority < 0) {
        return -EINVAL;
    } 
    if (!(st = (long *)rt_kstack_alloc(stack_size))) {
        return -ENOMEM;
    }
    if (lxrt_wdog_task[cpuid] && lxrt_wdog_task[cpuid] != task 
                     && priority == RT_SCHED_HIGHEST_PRIORITY) {
             rt_printk("Highest priority reserved for RTAI watchdog\n");
         return -EBUSY;
    }

    task->bstack = task->stack = (long *)(((unsigned long)st + stack_size - 0x10) & ~0xF);
    task->stack[0] = 0;
    task->uses_fpu = uses_fpu ? 1 : 0;
    task->runnable_on_cpus = cpuid;
    atomic_inc((atomic_t *)(tasks_per_cpu + cpuid));
    *(task->stack_bottom = st) = 0;
    task->magic = RT_TASK_MAGIC; 
    task->policy = 0;
    task->suspdepth = 1;
    task->state = (RT_SCHED_SUSPENDED | RT_SCHED_READY);
    task->owndres = 0;
    task->is_hard = 1;
    task->lnxtsk = 0;
    task->priority = task->base_priority = priority;
    task->prio_passed_to = 0;
    task->period = 0;
    task->resume_time = RT_TIME_END;
    task->periodic_resume_time = RT_TIME_END;
    task->queue.prev = &(task->queue);      
    task->queue.next = &(task->queue);      
    task->queue.task = task;
    task->msg_queue.prev = &(task->msg_queue);      
    task->msg_queue.next = &(task->msg_queue);      
    task->msg_queue.task = task;    
    task->msg = 0;  
    task->ret_queue.prev = &(task->ret_queue);
    task->ret_queue.next = &(task->ret_queue);
    task->ret_queue.task = NULL;
    task->tprev = task->tnext =
    task->rprev = task->rnext = task;
    task->blocked_on = NULL;        
    task->signal = signal;
    task->unblocked = 0;
    task->rt_signals = NULL;
    for (i = 0; i < RTAI_NR_TRAPS; i++) {
        task->task_trap_handler[i] = NULL;
    }
    task->linux_syscall_server = NULL;
    task->busy_time_align = 0;
    task->resync_frame = 0;
    task->ExitHook = 0;
    task->exectime[0] = task->exectime[1] = 0;
    task->system_data_ptr = 0;

    task->max_msg_size[0] = (long)rt_thread;
    task->max_msg_size[1] = data;
    init_arch_stack();

    flags = rt_global_save_flags_and_cli();
    task->next = 0;
    rt_linux_task.prev->next = task;
    task->prev = rt_linux_task.prev;
    rt_linux_task.prev = task;
    init_task_fpenv(task);
    rt_global_restore_flags(flags);

    task->resq.prev = task->resq.next = &task->resq;
    task->resq.task = NULL;

    return 0;
}

int rt_task_init(RT_TASK *task, void (*rt_thread)(long), long data,
            int stack_size, int priority, int uses_fpu,
            void(*signal)(void))
{
    return rt_task_init_cpuid(task, rt_thread, data, stack_size, priority, 
                 uses_fpu, signal, get_min_tasks_cpuid());
}

#else /* !USE_RTAI_TASKS */

int rt_task_init_cpuid(RT_TASK *task, void (*rt_thread)(long), long data, int stack_size, int priority, int uses_fpu, void(*signal)(void), unsigned int cpuid)
{
    return rt_kthread_init_cpuid(task, rt_thread, data, stack_size, priority, uses_fpu, signal, cpuid);
}

int rt_task_init(RT_TASK *task, void (*rt_thread)(long), long data, int stack_size, int priority, int uses_fpu, void(*signal)(void))
{
    return rt_kthread_init(task, rt_thread, data, stack_size, priority, uses_fpu, signal);
}

#endif /* USE_RTAI_TASKS */

RTAI_SYSCALL_MODE void rt_set_runnable_on_cpuid(RT_TASK *task, unsigned int cpuid)
{
    unsigned long flags;
    RT_TASK *linux_task;

    if (task->lnxtsk) {
        return;
    }

    if (cpuid >= NR_RT_CPUS) {
        cpuid = get_min_tasks_cpuid();
    } 
    flags = rt_global_save_flags_and_cli();
    switch (rt_smp_oneshot_timer[task->runnable_on_cpus] | 
        (rt_smp_oneshot_timer[cpuid] << 1)) {   
                case 1:
                        task->period = llimd(task->period, TIMER_FREQ, tuned.cpu_freq);
                        task->resume_time = llimd(task->resume_time, TIMER_FREQ, tuned.cpu_freq);
                        task->periodic_resume_time = llimd(task->periodic_resume_time, TIMER_FREQ, tuned.cpu_freq);
                        break;
                case 2:
                        task->period = llimd(task->period, tuned.cpu_freq, TIMER_FREQ);
                        task->resume_time = llimd(task->resume_time, tuned.cpu_freq, TIMER_FREQ);
                        task->periodic_resume_time = llimd(task->periodic_resume_time, tuned.cpu_freq, TIMER_FREQ);
            break;
    }
    if (!((task->prev)->next = task->next)) {
        rt_smp_linux_task[task->runnable_on_cpus].prev = task->prev;
    } else {
        (task->next)->prev = task->prev;
    }
    if ((task->state & RT_SCHED_DELAYED)) {
        rem_timed_task(task);
        task->runnable_on_cpus = cpuid;
        enq_timed_task(task);
    } else {
        task->runnable_on_cpus = cpuid;
    }
    task->next = 0;
    (linux_task = rt_smp_linux_task + cpuid)->prev->next = task;
    task->prev = linux_task->prev;
    linux_task->prev = task;
    rt_global_restore_flags(flags);
}


RTAI_SYSCALL_MODE void rt_set_runnable_on_cpus(RT_TASK *task, unsigned long run_on_cpus)
{
    int cpuid;

    if (task->lnxtsk) {
        return;
    }

#ifdef CONFIG_SMP
    run_on_cpus &= CPUMASK(cpu_online_map);
#else
    run_on_cpus = 1;
#endif
    cpuid = get_min_tasks_cpuid();
    if (!test_bit(cpuid, &run_on_cpus)) {
        cpuid = ffnz(run_on_cpus);
    }
    rt_set_runnable_on_cpuid(task, cpuid);
}


int rt_check_current_stack(void)
{
    DECLARE_RT_CURRENT;
    char *sp;

    ASSIGN_RT_CURRENT;
    if (rt_current != &rt_linux_task) {
        sp = get_stack_pointer();
        return (sp - (char *)(rt_current->stack_bottom));
    } else {
        return RT_RESEM_SUSPDEL;
    }
}


#define RR_YIELD() \
if (CONFIG_RTAI_ALLOW_RR && rt_current->policy > 0) { \
    if (rt_current->yield_time <= rt_times.tick_time) { \
        rt_current->rr_remaining = rt_current->rr_quantum; \
        if (rt_current->state == RT_SCHED_READY) { \
            RT_TASK *task; \
            task = rt_current->rnext; \
            while (rt_current->priority == task->priority) { \
                task = task->rnext; \
            } \
            if (task != rt_current->rnext) { \
                (rt_current->rprev)->rnext = rt_current->rnext; \
                (rt_current->rnext)->rprev = rt_current->rprev; \
                task->rprev = (rt_current->rprev = task->rprev)->rnext = rt_current; \
                rt_current->rnext = task; \
            } \
        } \
    } else { \
        rt_current->rr_remaining = rt_current->yield_time - rt_times.tick_time; \
    } \
} 

#define TASK_TO_SCHEDULE() \
do { \
    new_task = rt_linux_task.rnext; \
    if (CONFIG_RTAI_ALLOW_RR && new_task->policy > 0) { \
        new_task->yield_time = rt_times.tick_time + new_task->rr_remaining; \
    } \
} while (0)

#define RR_INTR_TIME(fire_shot) \
do { \
    fire_shot = 0; \
    prio = new_task->priority; \
    if (CONFIG_RTAI_ALLOW_RR && new_task->policy > 0) { \
        if (new_task->yield_time < rt_times.intr_time) { \
            rt_times.intr_time = new_task->yield_time; \
            fire_shot = 1; \
        } \
        } \
} while (0)

#define LOCK_LINUX(cpuid) \
    do { rt_switch_to_real_time(cpuid); } while (0)
#define UNLOCK_LINUX(cpuid) \
    do { rt_switch_to_linux(cpuid);     } while (0)

#define SAVE_LOCK_LINUX(cpuid) \
    do { sflags = rt_save_switch_to_real_time(cpuid); } while (0)
#define RESTORE_UNLOCK_LINUX(cpuid) \
    do { rt_restore_switch_to_linux(sflags, cpuid);   } while (0)

#ifdef LOCKED_LINUX_IN_IRQ_HANDLER
#define SAVE_LOCK_LINUX_IN_IRQ(cpuid)
#define RESTORE_UNLOCK_LINUX_IN_IRQ(cpuid)
#else
#define SAVE_LOCK_LINUX_IN_IRQ(cpuid)       LOCK_LINUX(cpuid)    
#define RESTORE_UNLOCK_LINUX_IN_IRQ(cpuid)  UNLOCK_LINUX(cpuid)
#endif

#if defined(CONFIG_RTAI_TASK_SWITCH_SIGNAL) && CONFIG_RTAI_TASK_SWITCH_SIGNAL

#define RTAI_TASK_SWITCH_SIGNAL() \
    do { \
        void (*signal)(void) = rt_current->signal; \
        if ((unsigned long)signal > MAXSIGNALS) { \
            (*signal)(); \
        } else if (signal) { \
            rt_current->signal = NULL; /* to avoid recursing */\
            rt_trigger_signal((long)signal, rt_current); \
            rt_current->signal = signal; \
        } \
    } while (0)
#else

#define RTAI_TASK_SWITCH_SIGNAL()

#endif
    
#if CONFIG_RTAI_MONITOR_EXECTIME

RTIME switch_time[NR_RT_CPUS];

#define SET_EXEC_TIME() \
    do { \
        RTIME now; \
        now = rdtsc(); \
        rt_current->exectime[0] += (now - switch_time[cpuid]); \
        switch_time[cpuid] = now; \
    } while (0)

#define RST_EXEC_TIME()  do { switch_time[cpuid] = rdtsc(); } while (0)

#else

#define SET_EXEC_TIME()
#define RST_EXEC_TIME()

#endif

#ifdef CONFIG_RTAI_WD
#define SAVE_PREV_TASK()  \
    do { lxrt_prev_task[cpuid] = rt_current; } while (0)
#else
#define SAVE_PREV_TASK()  do { } while (0)
#endif

void rt_do_force_soft(RT_TASK *rt_task)
{
    rt_global_cli();
    if (rt_task->state != RT_SCHED_READY) {
        rt_task->state &= ~RT_SCHED_READY;
                enq_ready_task(rt_task);
        RT_SCHEDULE(rt_task, rtai_cpuid());
    }
    rt_global_sti();
}

#define enq_soft_ready_task(ready_task) \
do { \
    RT_TASK *task = rt_smp_linux_task[cpuid].rnext; \
    if (ready_task == task) break; \
    task->rprev = (ready_task->rprev = task->rprev)->rnext = ready_task; \
    ready_task->rnext = task; \
} while (0)


#define pend_wake_up_hts(lnxtsk, cpuid) \
do { \
    wake_up_hts[cpuid].task[wake_up_hts[cpuid].in++ & (MAX_WAKEUP_SRQ - 1)] = lnxtsk; \
    hal_pend_uncond(wake_up_srq[cpuid].srq, cpuid); \
} while (0)


static inline void force_current_soft(RT_TASK *rt_current, int cpuid)
{
    struct task_struct *lnxtsk;
        void rt_schedule(void);
        rt_current->force_soft = 0;
    rt_current->state &= ~RT_SCHED_READY;;
    pend_wake_up_hts(lnxtsk = rt_current->lnxtsk, cpuid);
        (rt_current->rprev)->rnext = rt_current->rnext;
        (rt_current->rnext)->rprev = rt_current->rprev;
        rt_schedule();
        rt_current->is_hard = 0;
    if (rt_current->priority < BASE_SOFT_PRIORITY) {
        if (rt_current->priority == rt_current->base_priority) {
            rt_current->priority += BASE_SOFT_PRIORITY;
        }
    }
    if (rt_current->base_priority < BASE_SOFT_PRIORITY) {
        rt_current->base_priority += BASE_SOFT_PRIORITY;
    }
    rt_global_sti();
        hal_schedule_back_root(lnxtsk);
// now make it as if it was scheduled soft, the tail is cared in sys_lxrt.c
    rt_global_cli();
    LOCK_LINUX(cpuid);
    rt_current->state |= RT_SCHED_READY;
    rt_smp_current[cpuid] = rt_current;
        if (rt_current->state != RT_SCHED_READY) {
            lnxtsk->state = TASK_SOFTREALTIME;
        rt_schedule();
    } else {
        enq_soft_ready_task(rt_current);
    }
}

static RT_TASK *switch_rtai_tasks(RT_TASK *rt_current, RT_TASK *new_task, int cpuid)
{
    if (rt_current->lnxtsk) {
        unsigned long sflags;
#ifdef IPIPE_NOSTACK_FLAG
        ipipe_set_foreign_stack(&rtai_domain);
#endif
        SAVE_LOCK_LINUX(cpuid);
        rt_linux_task.prevp = rt_current;
        save_fpcr_and_enable_fpu(linux_cr0);
        if (new_task->uses_fpu) {
            save_fpenv(rt_linux_task.fpu_reg);
            fpu_task = new_task;
            restore_fpenv(fpu_task->fpu_reg);
        }
        RST_EXEC_TIME();
        SAVE_PREV_TASK();
        rt_exchange_tasks(rt_smp_current[cpuid], new_task);
        restore_fpcr(linux_cr0);
        RESTORE_UNLOCK_LINUX(cpuid);
#ifdef IPIPE_NOSTACK_FLAG
        ipipe_clear_foreign_stack(&rtai_domain);
#endif
        if (rt_linux_task.nextp != rt_current) {
            return rt_linux_task.nextp;
        }
    } else {
        if (new_task->lnxtsk) {
            rt_linux_task.nextp = new_task;
            new_task = rt_linux_task.prevp;
            if (fpu_task != &rt_linux_task) {
                save_fpenv(fpu_task->fpu_reg);
                fpu_task = &rt_linux_task;
                restore_fpenv(fpu_task->fpu_reg);
            }
        } else if (new_task->uses_fpu && fpu_task != new_task) {
            save_fpenv(fpu_task->fpu_reg);
            fpu_task = new_task;
            restore_fpenv(fpu_task->fpu_reg);
        }
        SET_EXEC_TIME();
        SAVE_PREV_TASK();
        rt_exchange_tasks(rt_smp_current[cpuid], new_task);
    }
    RTAI_TASK_SWITCH_SIGNAL();
    return NULL;
}

#define lxrt_context_switch(prev, next, cpuid) \
    do { \
        SAVE_PREV_TASK(); \
        _lxrt_context_switch(prev, next, cpuid); barrier(); \
        RTAI_TASK_SWITCH_SIGNAL(); \
    } while (0)


#ifdef USE_LINUX_TIMER

#define CHECK_LINUX_TIME() \
    if (rt_times.linux_time < rt_times.intr_time) { \
        rt_times.intr_time = rt_times.linux_time; \
        fire_shot = 1; \
        break; \
    }

#define SET_PEND_LINUX_TIMER_SHOT() \
do { \
    if (rt_times.tick_time >= rt_times.linux_time) { \
        if (rt_times.linux_tick > 0) { \
            rt_times.linux_time += rt_times.linux_tick; \
        } else { \
            rt_times.linux_time = RT_TIME_END; \
        } \
        update_linux_timer(cpuid); \
    } \
} while (0)

#else

#define CHECK_LINUX_TIME()

#define SET_PEND_LINUX_TIMER_SHOT()

#endif


#define SET_NEXT_TIMER_SHOT(fire_shot) \
do { \
    fire_shot = 0; \
    prio = new_task->priority; \
    if (CONFIG_RTAI_ALLOW_RR && new_task->policy > 0) { \
        if (new_task->yield_time < rt_times.intr_time) { \
            rt_times.intr_time = new_task->yield_time; \
            fire_shot = 1; \
        } \
        } \
    task = &rt_linux_task; \
    while ((task = task->tnext) != &rt_linux_task && task->resume_time < rt_times.intr_time) { \
        if (task->priority <= prio) { \
            rt_times.intr_time = task->resume_time; \
            fire_shot = 1; \
            break; \
        } \
    } \
} while (0) 

#define IF_GOING_TO_LINUX_CHECK_TIMER_SHOT(fire_shot) \
do { \
    if (prio == RT_SCHED_LINUX_PRIORITY) { \
        CHECK_LINUX_TIME(); \
        if (!timer_shot_fired) {\
            fire_shot = 1; \
        } \
    } \
} while (0)

static int oneshot_span;
static int satdlay;

#define ONESHOT_DELAY(SHOT_FIRED) \
do { \
    if (!(SHOT_FIRED)) { \
        RTIME span; \
        if (unlikely((span = rt_times.intr_time - rt_time_h) > oneshot_span)) { \
            rt_times.intr_time = rt_time_h + oneshot_span; \
            delay = satdlay; \
        } else { \
            delay = (int)span - tuned.latency; \
        } \
    } else { \
        delay = (int)(rt_times.intr_time - rt_time_h) - tuned.latency; \
    } \
} while (0)

#if 1

static void rt_timer_handler(void);

#define FIRE_NEXT_TIMER_SHOT(SHOT_FIRED) \
do { \
if (fire_shot) { \
    int delay; \
    ONESHOT_DELAY(SHOT_FIRED); \
    if (delay > tuned.setup_time_TIMER_CPUNIT) { \
        rt_set_timer_delay(imuldiv(delay, TIMER_FREQ, tuned.cpu_freq));\
        timer_shot_fired = 1; \
    } else { \
        rt_times.intr_time = rt_time_h + tuned.setup_time_TIMER_CPUNIT;\
        timer_shot_fired = -1;\
    } \
} \
} while (0)

#define CALL_TIMER_HANDLER() \
    do { if (timer_shot_fired < 0) rt_timer_handler(); } while (0)

#define REDO_TIMER_HANDLER() \
    do { if (timer_shot_fired < 0) goto redo_timer_handler; } while (0)

#define FIRE_IMMEDIATE_LINUX_TIMER_SHOT() \
do { \
    LOCK_LINUX(cpuid); \
    rt_timer_handler(); \
    UNLOCK_LINUX(cpuid); \
} while (0)

#else

#define FIRE_NEXT_TIMER_SHOT(CHECK_SPAN) \
do { \
if (fire_shot) { \
    int delay; \
    ONESHOT_DELAY(CHECK_SPAN); \
    if (delay > tuned.setup_time_TIMER_CPUNIT) { \
        rt_set_timer_delay(imuldiv(delay, TIMER_FREQ, tuned.cpu_freq));\
    } else { \
        rt_set_timer_delay(tuned.setup_time_TIMER_UNIT); \
        rt_times.intr_time = rt_time_h + tuned.setup_time_TIMER_CPUNIT;\
    } \
    timer_shot_fired = 1; \
} \
} while (0)

#define CALL_TIMER_HANDLER()

#define REDO_TIMER_HANDLER()

#endif

#ifdef CONFIG_SMP
static void rt_schedule_on_schedule_ipi(void)
{
    RT_TASK *rt_current, *task, *new_task;
    int cpuid;

    rt_current = rt_smp_current[cpuid = rtai_cpuid()];

    sched_get_global_lock(cpuid);
    RR_YIELD();
    if (oneshot_running) {
        int prio, fire_shot;

        rt_time_h = rdtsc() + rt_half_tick;
        wake_up_timed_tasks(cpuid);
        TASK_TO_SCHEDULE();

        SET_NEXT_TIMER_SHOT(fire_shot);
        sched_release_global_lock(cpuid);
        IF_GOING_TO_LINUX_CHECK_TIMER_SHOT(fire_shot);
        FIRE_NEXT_TIMER_SHOT(timer_shot_fired);
    } else {
        TASK_TO_SCHEDULE();
        sched_release_global_lock(cpuid);
    }

    if (new_task != rt_current) {
        if (rt_scheduling[cpuid].locked) {
            rt_scheduling[cpuid].rqsted = 1;
            goto sched_exit;
        }
        if (USE_RTAI_TASKS && (!new_task->lnxtsk || !rt_current->lnxtsk)) {
            if (!(new_task = switch_rtai_tasks(rt_current, new_task, cpuid))) {
                goto sched_exit;
            }
        }
        if (new_task->is_hard > 0 || rt_current->is_hard > 0) {
            struct task_struct *prev;
            unsigned long sflags;
            if (rt_current->is_hard <= 0) {
                SAVE_LOCK_LINUX_IN_IRQ(cpuid);
                rt_linux_task.lnxtsk = prev = current;
                RST_EXEC_TIME();
            } else {
                sflags = rtai_linux_context[cpuid].sflags;
                prev = rt_current->lnxtsk;
                SET_EXEC_TIME();
            }
            rt_smp_current[cpuid] = new_task;
            lxrt_context_switch(prev, new_task->lnxtsk, cpuid);
            if (rt_current->is_hard <= 0) {
                RESTORE_UNLOCK_LINUX_IN_IRQ(cpuid);
            } else if (lnxtsk_uses_fpu(prev)) {
                restore_fpu(prev);
            }
        }
    }
sched_exit:
    CALL_TIMER_HANDLER();
#if CONFIG_RTAI_BUSY_TIME_ALIGN
    if (rt_current->busy_time_align) {
        rt_current->busy_time_align = 0;
        while(rdtsc() < rt_current->resume_time);
    }
#endif
}
#endif

void rt_schedule(void)
{
    RT_TASK *rt_current, *task, *new_task;
    int cpuid;

    rt_current = rt_smp_current[cpuid = rtai_cpuid()];

    RR_YIELD();
    if (oneshot_running) {
        int prio, fire_shot;

        rt_time_h = rdtsc() + rt_half_tick;
        wake_up_timed_tasks(cpuid);
        TASK_TO_SCHEDULE();

        SET_NEXT_TIMER_SHOT(fire_shot);
        sched_release_global_lock(cpuid);
        IF_GOING_TO_LINUX_CHECK_TIMER_SHOT(fire_shot);
        FIRE_NEXT_TIMER_SHOT(timer_shot_fired);
    } else {
        TASK_TO_SCHEDULE();
        sched_release_global_lock(cpuid);
    }

    if (new_task != rt_current) {
        if (rt_scheduling[cpuid].locked) {
            rt_scheduling[cpuid].rqsted = 1;
            goto sched_exit;
        }
        if (USE_RTAI_TASKS && (!new_task->lnxtsk || !rt_current->lnxtsk)) {
            if (!(new_task = switch_rtai_tasks(rt_current, new_task, cpuid))) {
                goto sched_exit;
            }
        }
        rt_smp_current[cpuid] = new_task;
        if (new_task->is_hard > 0 || rt_current->is_hard > 0) {
            struct task_struct *prev;
            unsigned long sflags;
            if (rt_current->is_hard <= 0) {
                SAVE_LOCK_LINUX(cpuid);
                rt_linux_task.lnxtsk = prev = current;
                RST_EXEC_TIME();
            } else {
                sflags = rtai_linux_context[cpuid].sflags;
                prev = rt_current->lnxtsk;
                SET_EXEC_TIME();
            }
            lxrt_context_switch(prev, new_task->lnxtsk, cpuid);
            if (rt_current->is_hard <= 0) {
                RESTORE_UNLOCK_LINUX(cpuid);
                if (rt_current->state != RT_SCHED_READY) {
                    goto sched_soft;
                }
            } else {
                if (lnxtsk_uses_fpu(prev)) {
                    restore_fpu(prev);
                }
                if (rt_current->force_soft) {
                    force_current_soft(rt_current, cpuid);
                }
            }
        } else {
sched_soft:
            CALL_TIMER_HANDLER();
            UNLOCK_LINUX(cpuid);
            rtai_sti();

#ifdef CONFIG_RTAI_ALIGN_LINUX_PRIORITY
            if (current->rtai_tskext(TSKEXT0) && (current->policy == SCHED_FIFO || current->policy == SCHED_RR)) {
                int rt_priority;
                if ((rt_priority = ((RT_TASK *)current->rtai_tskext(TSKEXT0))->priority) >= BASE_SOFT_PRIORITY) {
                    rt_priority -= BASE_SOFT_PRIORITY;
                }
                if ((rt_priority = (MAX_LINUX_RTPRIO - rt_priority)) < 1) {
                    rt_priority = 1;
                }
                if (rt_priority != current->rt_priority) {
                    rtai_set_linux_task_priority(current, current->policy, rt_priority);
                }
            }
#endif

            hal_test_and_fast_flush_pipeline(cpuid);
            NON_RTAI_SCHEDULE(cpuid);
            rt_global_cli();
            rt_current->state = (rt_current->state & ~RT_SCHED_SFTRDY) | RT_SCHED_READY;
            LOCK_LINUX(cpuid);
            enq_soft_ready_task(rt_current);
            rt_smp_current[cpuid] = rt_current;
            return;
        }
    }
sched_exit:
    CALL_TIMER_HANDLER();
#if CONFIG_RTAI_BUSY_TIME_ALIGN
    if (rt_current->busy_time_align) {
        rt_current->busy_time_align = 0;
        while(rdtsc() < rt_current->resume_time);
    }
#endif
    sched_get_global_lock(cpuid);
}

RTAI_SYSCALL_MODE void rt_spv_RMS(int cpuid)
{
    RT_TASK *task;
    int prio;
    if (cpuid < 0 || cpuid >= num_online_cpus()) {
        cpuid = rtai_cpuid();
    }
    prio = 0;
    task = &rt_linux_task;
    while ((task = task->next)) {
        RT_TASK *task, *htask;
        RTIME period;
        htask = 0;
        task = &rt_linux_task;
        period = RT_TIME_END;
        while ((task = task->next)) {
            if (task->priority >= 0 && task->policy >= 0 && task->period && task->period < period) {
                period = (htask = task)->period;
            }
        }
        if (htask) {
            htask->priority = -1;
            htask->base_priority = prio++;
        } else {
            goto ret;
        }
    }
ret:    task = &rt_linux_task;
    while ((task = task->next)) {
        if (task->priority < 0) {
            task->priority = task->base_priority;
        }
    }
    return;
}


void rt_sched_lock(void)
{
    unsigned long flags;
    int cpuid;

    rtai_save_flags_and_cli(flags);
    if (!rt_scheduling[cpuid = rtai_cpuid()].locked++) {
        rt_scheduling[cpuid].rqsted = 0;
    }
    rtai_restore_flags(flags);
}

#define SCHED_UNLOCK_SCHEDULE(cpuid) \
    do { \
        rt_scheduling[cpuid].rqsted = 0; \
        sched_get_global_lock(cpuid); \
        rt_schedule(); \
        sched_release_global_lock(cpuid); \
    } while (0)


void rt_sched_unlock(void)
{
    unsigned long flags;
    int cpuid;

    rtai_save_flags_and_cli(flags);
    if (rt_scheduling[cpuid = rtai_cpuid()].locked && !(--rt_scheduling[cpuid].locked)) {
        if (rt_scheduling[cpuid].rqsted > 0) {
            SCHED_UNLOCK_SCHEDULE(cpuid);
        }
    } else {
//      rt_printk("*** TOO MANY SCHED_UNLOCK ***\n");
    }
    rtai_restore_flags(flags);
}


void rtai_handle_isched_lock (int cpuid) /* Called with interrupts off */
{
    SCHED_UNLOCK_SCHEDULE(cpuid);
}


void *rt_get_lxrt_fun_entry(int index);
static inline void sched_sem_signal(SEM *sem)
{
    ((RTAI_SYSCALL_MODE void (*)(SEM *, ...))rt_get_lxrt_fun_entry(SEM_SIGNAL))(sem);
}

int clr_rtext(RT_TASK *task)
{
    DECLARE_RT_CURRENT;
    unsigned long flags;
    QUEUE *q;

    if (task->magic != RT_TASK_MAGIC || task->priority == RT_SCHED_LINUX_PRIORITY) {
        return -EINVAL;
    }

    flags = rt_global_save_flags_and_cli();
    ASSIGN_RT_CURRENT;
    if (!task_owns_sems(task) || task == rt_current || rt_current->priority == RT_SCHED_LINUX_PRIORITY) {
        call_exit_handlers(task);
        rem_timed_task(task);
        if (task->blocked_on) {
            if (task->state & (RT_SCHED_SEMAPHORE | RT_SCHED_SEND | RT_SCHED_RPC | RT_SCHED_RETURN)) {
                (task->queue.prev)->next = task->queue.next;
                (task->queue.next)->prev = task->queue.prev;
                if (task->state & RT_SCHED_SEMAPHORE) {
                    SEM *sem = (SEM *)(task->blocked_on);
                    if (++sem->count > 1 && sem->type) {
                        sem->count = 1;
                    }
                }
            } else if (task->state & RT_SCHED_MBXSUSP) {
                MBX *mbx = (MBX *)task->blocked_on;
                mbx->waiting_task = NULL;
                sched_sem_signal(!mbx->frbs ? &mbx->sndsem : &mbx->rcvsem);
            }
        }
        q = &(task->msg_queue);
        while ((q = q->next) != &(task->msg_queue)) {
            rem_timed_task(q->task);
            if ((q->task)->state != RT_SCHED_READY && ((q->task)->state &= ~(RT_SCHED_SEND | RT_SCHED_RPC | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
                enq_ready_task(q->task);
            }       
            (q->task)->blocked_on = RTP_OBJREM;
        }       
                q = &(task->ret_queue);
                while ((q = q->next) != &(task->ret_queue)) {
            rem_timed_task(q->task);
                        if ((q->task)->state != RT_SCHED_READY && ((q->task)->state &= ~(RT_SCHED_RETURN | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
                enq_ready_task(q->task);
            }       
            (q->task)->blocked_on = RTP_OBJREM;
                }
        if (!((task->prev)->next = task->next)) {
            rt_smp_linux_task[task->runnable_on_cpus].prev = task->prev;
        } else {
            (task->next)->prev = task->prev;
        }
        if (rt_smp_fpu_task[task->runnable_on_cpus] == task) {
            rt_smp_fpu_task[task->runnable_on_cpus] = rt_smp_linux_task + task->runnable_on_cpus;;
        }
        if (!task->lnxtsk) {
            frstk_srq.mp[frstk_srq.in++ & (MAX_FRESTK_SRQ - 1)] = task->stack_bottom;
            rt_pend_linux_srq(frstk_srq.srq);
        }
        task->magic = 0;
        rem_ready_task(task);
        task->state = 0;
        atomic_dec((void *)(tasks_per_cpu + task->runnable_on_cpus));
        if (task == rt_current) {
            rt_schedule();
        }
    } else {
        task->suspdepth = -0x7FFFFFFF;
    }
    rt_global_restore_flags(flags);
    return 0;
}


int rt_task_delete(RT_TASK *task)
{
    if (!clr_rtext(task)) {
        if (task->lnxtsk) {
            start_stop_kthread(task, 0, 0, 0, 0, 0, 0);
        }
    }
    return 0;
}


int rt_get_timer_cpu(void)
{
    return 1;
}


static void rt_timer_handler(void)
{
    RT_TASK *rt_current, *task, *new_task;
    int cpuid;

    DO_TIMER_PROPER_OP();
    rt_current = rt_smp_current[cpuid = rtai_cpuid()];

redo_timer_handler:

    rt_times.tick_time = oneshot_timer ? rdtsc() : rt_times.intr_time;
    rt_time_h = rt_times.tick_time + rt_half_tick;
    SET_PEND_LINUX_TIMER_SHOT();

    sched_get_global_lock(cpuid);
    RR_YIELD();
    wake_up_timed_tasks(cpuid);
    TASK_TO_SCHEDULE();

    if (oneshot_timer) {
        int prio, fire_shot;

        timer_shot_fired = 0;
        rt_times.intr_time = RT_TIME_END;

        SET_NEXT_TIMER_SHOT(fire_shot);
        sched_release_global_lock(cpuid);
        IF_GOING_TO_LINUX_CHECK_TIMER_SHOT(fire_shot);
        FIRE_NEXT_TIMER_SHOT(0);
    } else {
        sched_release_global_lock(cpuid);
        rt_times.intr_time += rt_times.periodic_tick;
                rt_set_timer_delay(0);
    }

    if (new_task != rt_current) {
        if (rt_scheduling[cpuid].locked) {
            rt_scheduling[cpuid].rqsted = 1;
            goto sched_exit;
        }
        if (USE_RTAI_TASKS && (!new_task->lnxtsk || !rt_current->lnxtsk)) {
            if (!(new_task = switch_rtai_tasks(rt_current, new_task, cpuid))) {
                goto sched_exit;
            }
        }
        if (new_task->is_hard > 0 || rt_current->is_hard > 0) {
            struct task_struct *prev;
            unsigned long sflags;
            if (rt_current->is_hard <= 0) {
                SAVE_LOCK_LINUX_IN_IRQ(cpuid);
                rt_linux_task.lnxtsk = prev = current;
                RST_EXEC_TIME();
            } else {
                sflags = rtai_linux_context[cpuid].sflags;
                prev = rt_current->lnxtsk;
                SET_EXEC_TIME();
            }
            rt_smp_current[cpuid] = new_task;
            lxrt_context_switch(prev, new_task->lnxtsk, cpuid);
            if (rt_current->is_hard <= 0) {
                RESTORE_UNLOCK_LINUX_IN_IRQ(cpuid);
            } else if (lnxtsk_uses_fpu(prev)) {
                restore_fpu(prev);
            }
        }
        }
sched_exit:
    REDO_TIMER_HANDLER();
    return;
    goto redo_timer_handler;
}


#if defined(USE_LINUX_TIMER) && !defined(CONFIG_GENERIC_CLOCKEVENTS)

static irqreturn_t recover_jiffies(int irq, void *dev_id, struct pt_regs *regs) 
{
    rt_global_cli();
    if (linux_times->tick_time >= linux_times->linux_time) {
        linux_times->linux_time += linux_times->linux_tick;
        update_linux_timer(rtai_cpuid());
    }
    rt_global_sti();
    return RTAI_LINUX_IRQ_HANDLED;
} 

#define REQUEST_RECOVER_JIFFIES()  rt_request_linux_irq(TIMER_8254_IRQ, recover_jiffies, "rtai_jif_chk", recover_jiffies)

#define RELEASE_RECOVER_JIFFIES(timer)  rt_free_linux_irq(TIMER_8254_IRQ, recover_jiffies)

#else 

#define REQUEST_RECOVER_JIFFIES()

#define RELEASE_RECOVER_JIFFIES()

#endif


int rt_is_hard_timer_running(void) 
{ 
    return rt_sched_timed;
}


void rt_set_periodic_mode(void) 
{ 
    int cpuid;
    stop_rt_timer();
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        oneshot_timer = oneshot_running = 0;
    }
}


void rt_set_oneshot_mode(void)
{ 
    int cpuid;
    stop_rt_timer();
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        oneshot_timer = 1;
    }
}


#if defined(CONFIG_RTAI_RTC_FREQ) && CONFIG_RTAI_RTC_FREQ >= 2

#ifdef CONFIG_SMP

RTAI_SYSCALL_MODE RTIME start_rt_timer(int period)
{
    int cpuid;
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        oneshot_timer = oneshot_running = 0;
        rt_smp_times[cpuid].linux_tick    = 0;
        rt_smp_times[cpuid].tick_time     = 0;
        rt_smp_times[cpuid].intr_time     = 0;
        rt_smp_times[cpuid].linux_time    = 0;
        rt_smp_times[cpuid].periodic_tick = 1;
        tuned.timers_tol[cpuid] = rt_half_tick = 0;
        rt_time_h = 0;
    }
    linux_times = rt_smp_times;
    rt_request_irq(RTAI_APIC_TIMER_IPI, (void *)rt_timer_handler, NULL, 0);
    rt_request_rtc(CONFIG_RTAI_RTC_FREQ, NULL);
    rt_sched_timed = 1;
    rt_gettimeorig(NULL);
    return 1LL;
}


void stop_rt_timer(void)
{
    if (rt_sched_timed) {
        int cpuid;
        rt_sched_timed = 0;
        rt_release_rtc();
        rt_release_irq(RTAI_APIC_TIMER_IPI);
        for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
            rt_time_h = RT_TIME_END;
            oneshot_running = 0;
        }
    }
}

#else /* !CONFIG_SMP */

RTAI_SYSCALL_MODE RTIME start_rt_timer(int period)
{
    int const cpuid = 0;
    oneshot_timer = oneshot_running = 0;
    rt_smp_times[cpuid].linux_tick    = 0;
    rt_smp_times[cpuid].tick_time     = 0;
    rt_smp_times[cpuid].intr_time     = 0;
    rt_smp_times[cpuid].linux_time    = 0;
    rt_smp_times[cpuid].periodic_tick = 1;
    tuned.timers_tol[0] = rt_half_tick = 0;
    rt_time_h = 0;
    linux_times = rt_smp_times;
    rt_request_rtc(CONFIG_RTAI_RTC_FREQ, (void *)rt_timer_handler);
    rt_sched_timed = 1;
    rt_gettimeorig(NULL);
    return 1LL;
}

void stop_rt_timer(void)
{
    if (rt_sched_timed) {
        rt_sched_timed = 0;
        rt_release_rtc();
        rt_time_h = RT_TIME_END;
        rt_smp_oneshot_timer[0] = 0;
    }
}

#endif /* CONFIG_SMP */

RTAI_SYSCALL_MODE void start_rt_apic_timers(struct apic_timer_setup_data *setup_data, unsigned int rcvr_jiffies_cpuid)
{
    start_rt_timer(0);
}

#else /* !CONFIG_RTAI_RTC_FREQ */

#ifdef CONFIG_GENERIC_CLOCKEVENTS

#include <linux/clockchips.h>
#include <linux/ipipe_tickdev.h>

extern void *rt_linux_hrt_set_mode;
extern void *rt_linux_hrt_next_shot;

static void _rt_linux_hrt_set_mode(enum clock_event_mode mode, struct ipipe_tick_device *hrt_dev)
{
    int cpuid = rtai_cpuid();

    if (mode == CLOCK_EVT_MODE_ONESHOT || mode == CLOCK_EVT_MODE_SHUTDOWN) {
        rt_times.linux_tick = 0;
    } else if (mode == CLOCK_EVT_MODE_PERIODIC) {
        rt_times.linux_tick = nano2count_cpuid((1000000000 + HZ/2)/HZ, cpuid);
    }
}

static int _rt_linux_hrt_next_shot(unsigned long deltat, struct ipipe_tick_device *hrt_dev)
{
    int cpuid = rtai_cpuid();
    RTIME linux_time;

    deltat = nano2count_cpuid(deltat, cpuid);
    linux_time = rt_get_time_cpuid(cpuid) + deltat;
    deltat = deltat > (tuned.setup_time_TIMER_CPUNIT + tuned.latency) ? imuldiv(deltat - tuned.latency, TIMER_FREQ, tuned.cpu_freq) : 0;

    rtai_cli();
    rt_times.linux_time = linux_time;
    if (oneshot_running) {
        if (linux_time < rt_times.intr_time) {
            if (deltat > 0) {
                rt_times.intr_time = linux_time;
                rt_set_timer_delay(deltat);
                timer_shot_fired = 1;
            } else {
                rt_times.linux_time = RT_TIME_END;
                        update_linux_timer(cpuid);
            }
        }
    }
    rtai_sti();
    return 0;
}

#endif /* CONFIG_GENERIC_CLOCKEVENTS */

#ifdef CONFIG_SMP

RTAI_SYSCALL_MODE void start_rt_apic_timers(struct apic_timer_setup_data *setup_data, unsigned int rcvr_jiffies_cpuid)
{
    unsigned long flags, cpuid;

    rt_request_apic_timers(rt_timer_handler, setup_data);
    flags = rt_global_save_flags_and_cli();
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        if (setup_data[cpuid].mode > 0) {
            oneshot_timer = oneshot_running = 0;
            tuned.timers_tol[cpuid] = rt_half_tick = (rt_times.periodic_tick + 1)>>1;
        } else {
            oneshot_timer = oneshot_running = 1;
            tuned.timers_tol[cpuid] = rt_half_tick = (tuned.latency + 1)>>1;
        }
        rt_time_h = rt_times.tick_time + rt_half_tick;
        timer_shot_fired = 1;
    }
    rt_sched_timed = 1;
    linux_times = rt_smp_times + (rcvr_jiffies_cpuid < NR_RT_CPUS ? rcvr_jiffies_cpuid : 0);
    rt_global_restore_flags(flags);
}


RTAI_SYSCALL_MODE RTIME start_rt_timer(int period)
{
    int cpuid;
    struct apic_timer_setup_data setup_data[NR_RT_CPUS];
    if (period <= 0) {
        period = 0;
        rt_set_oneshot_mode();
    }
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        setup_data[cpuid].mode = oneshot_timer ? 0 : 1;
        setup_data[cpuid].count = count2nano(period);
    }
    start_rt_apic_timers(setup_data, rtai_cpuid());
    rt_gettimeorig(NULL);
    return setup_data[0].mode ? setup_data[0].count : period;
}


void stop_rt_timer(void)
{
    if (rt_sched_timed) {
        int cpuid; 
        rt_sched_timed = 0;
        rt_free_apic_timers();
        for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
            rt_time_h = RT_TIME_END;
            oneshot_running = 0;
        }
    }
}

#else /* !CONFIG_SMP */

#ifndef TIMER_TYPE
#define TIMER_TYPE  1
#endif

RTAI_SYSCALL_MODE RTIME start_rt_timer(int period)
{
#define cpuid 0
#undef rt_times

        unsigned long flags;
    if (period <= 0) {
        period = 0;
        rt_set_oneshot_mode();
    }
        flags = rt_global_save_flags_and_cli();
        if (oneshot_timer) {
        rt_request_timer(rt_timer_handler, 0, TIMER_TYPE);
                tuned.timers_tol[0] = rt_half_tick = (tuned.latency + 1)>>1;
                oneshot_running = timer_shot_fired = 1;
        } else {
        rt_request_timer(rt_timer_handler, !TIMER_TYPE && period > LATCH ? LATCH: period, TIMER_TYPE);
                tuned.timers_tol[0] = rt_half_tick = (rt_times.periodic_tick + 1)>>1;
        }
    rt_sched_timed = 1;
    rt_smp_times[cpuid].linux_tick    = rt_times.linux_tick;
    rt_smp_times[cpuid].tick_time     = rt_times.tick_time;
    rt_smp_times[cpuid].intr_time     = rt_times.intr_time;
    rt_smp_times[cpuid].linux_time    = rt_times.linux_time;
    rt_smp_times[cpuid].periodic_tick = rt_times.periodic_tick;
        rt_time_h = rt_times.tick_time + rt_half_tick;
    linux_times = rt_smp_times;
        rt_global_restore_flags(flags);
    REQUEST_RECOVER_JIFFIES();
    rt_gettimeorig(NULL);
        return period;

#undef cpuid
#define rt_times (rt_smp_times[cpuid])
}


RTAI_SYSCALL_MODE void start_rt_apic_timers(struct apic_timer_setup_data *setup_mode, unsigned int rcvr_jiffies_cpuid)
{
    int cpuid, period;

    period = 0;
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        period += setup_mode[cpuid].mode;
    }
    if (period == NR_RT_CPUS) {
        period = 2000000000;
        for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
            if (setup_mode[cpuid].count < period) {
                period = setup_mode[cpuid].count;
            }
        }
        start_rt_timer(nano2count(period));
    } else {
        rt_set_oneshot_mode();
        start_rt_timer(0);
    }
}


void stop_rt_timer(void)
{
    if (rt_sched_timed) {
        rt_sched_timed = 0;
        RELEASE_RECOVER_JIFFIES();
        rt_free_timer();
        rt_time_h = RT_TIME_END;
        rt_smp_oneshot_timer[0] = 0;
    }
}

#endif /* CONFIG_SMP */

#endif /* CONFIG_RTAI_RTC_FREQ */

int rt_sched_type(void)
{
    return RT_SCHED_MUP;
}


RTAI_SYSCALL_MODE int rt_hard_timer_tick_count(void)
{
    int cpuid = rtai_cpuid();
    if (rt_sched_timed) {
        return oneshot_timer ? 0 : rt_smp_times[cpuid].periodic_tick;
    }
    return -1;
}


RTAI_SYSCALL_MODE int rt_hard_timer_tick_count_cpuid(int cpuid)
{
    if (rt_sched_timed) {
        return oneshot_timer ? 0 : rt_smp_times[cpuid].periodic_tick;
    }
    return -1;
}


RT_TRAP_HANDLER rt_set_task_trap_handler( RT_TASK *task, unsigned int vec, RT_TRAP_HANDLER handler)
{
    RT_TRAP_HANDLER old_handler;

    if (!task || (vec >= RTAI_NR_TRAPS)) {
        return (RT_TRAP_HANDLER) -EINVAL;
    }
    old_handler = task->task_trap_handler[vec];
    task->task_trap_handler[vec] = handler;
    return old_handler;
}

static int OneShot = CONFIG_RTAI_ONE_SHOT;
RTAI_MODULE_PARM(OneShot, int);

static int Latency = TIMER_LATENCY;
RTAI_MODULE_PARM(Latency, int);

static int SetupTimeTIMER = TIMER_SETUP_TIME;
RTAI_MODULE_PARM(SetupTimeTIMER, int);

extern void krtai_objects_release(void);

static void frstk_srq_handler(void)
{
        while (frstk_srq.out != frstk_srq.in) {
        rt_kstack_free(frstk_srq.mp[frstk_srq.out++ & (MAX_FRESTK_SRQ - 1)]);
    }
}

static void nihil(void) { };
struct rt_fun_entry rt_fun_lxrt[MAX_LXRT_FUN];

void reset_rt_fun_entries(struct rt_native_fun_entry *entry)
{
    while (entry->fun.fun) {
        if (entry->index >= MAX_LXRT_FUN) {
            rt_printk("*** RESET ENTRY %d FOR USER SPACE CALLS EXCEEDS ALLOWD TABLE SIZE %d, NOT USED ***\n", entry->index, MAX_LXRT_FUN);
        } else {
            rt_fun_lxrt[entry->index] = (struct rt_fun_entry){ 1, nihil };
        }
        entry++;
        }
}

int set_rt_fun_entries(struct rt_native_fun_entry *entry)
{
    int error;
    error = 0;
    while (entry->fun.fun) {
        if (rt_fun_lxrt[entry->index].fun != nihil) {
            rt_printk("*** SUSPICIOUS ENTRY ASSIGNEMENT FOR USER SPACE CALL AT %d, DUPLICATED INDEX OR REPEATED INITIALIZATION ***\n", entry->index);
            error = -1;
        } else if (entry->index >= MAX_LXRT_FUN) {
            rt_printk("*** ASSIGNEMENT ENTRY %d FOR USER SPACE CALLS EXCEEDS ALLOWED TABLE SIZE %d, NOT USED ***\n", entry->index, MAX_LXRT_FUN);
            error = -1;
        } else {
            rt_fun_lxrt[entry->index] = entry->fun;
        }
        entry++;
        }
    if (error) {
        reset_rt_fun_entries(entry);
    }
    return 0;
}

void *rt_get_lxrt_fun_entry(int index) {
    return rt_fun_lxrt[index].fun;
}

static void lxrt_killall (void)
{
    int cpuid;
    
    stop_rt_timer();
    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        while (rt_linux_task.next) {
            rt_task_delete(rt_linux_task.next);
        }
    }
}

static int lxrt_notify_reboot (struct notifier_block *nb, unsigned long event, void *p)
{
    switch (event) {
        case SYS_DOWN:
        case SYS_HALT:
        case SYS_POWER_OFF:
        /* FIXME: this is far too late. */
        printk("LXRT: REBOOT NOTIFIED -- KILLING TASKS\n");
        lxrt_killall();
    }
    return NOTIFY_DONE;
}

/* ++++++++++++++++++++++++++ TIME CONVERSIONS +++++++++++++++++++++++++++++ */

RTAI_SYSCALL_MODE RTIME count2nano(RTIME counts)
{
    int sign;

    if (counts >= 0) {
        sign = 1;
    } else {
        sign = 0;
        counts = - counts;
    }
    counts = oneshot_timer_cpuid ?
         llimd(counts, 1000000000, tuned.cpu_freq):
         llimd(counts, 1000000000, TIMER_FREQ);
    return sign ? counts : - counts;
}


RTAI_SYSCALL_MODE RTIME nano2count(RTIME ns)
{
    int sign;

    if (ns >= 0) {
        sign = 1;
    } else {
        sign = 0;
        ns = - ns;
    }
    ns =  oneshot_timer_cpuid ?
          llimd(ns, tuned.cpu_freq, 1000000000) :
          llimd(ns, TIMER_FREQ, 1000000000);
    return sign ? ns : - ns;
}

RTAI_SYSCALL_MODE RTIME count2nano_cpuid(RTIME counts, unsigned int cpuid)
{
    int sign;

    if (counts >= 0) {
        sign = 1;
    } else {
        sign = 0;
        counts = - counts;
    }
    counts = oneshot_timer ?
         llimd(counts, 1000000000, tuned.cpu_freq):
         llimd(counts, 1000000000, TIMER_FREQ);
    return sign ? counts : - counts;
}


RTAI_SYSCALL_MODE RTIME nano2count_cpuid(RTIME ns, unsigned int cpuid)
{
    int sign;

    if (ns >= 0) {
        sign = 1;
    } else {
        sign = 0;
        ns = - ns;
    }
    ns =  oneshot_timer ?
          llimd(ns, tuned.cpu_freq, 1000000000) :
          llimd(ns, TIMER_FREQ, 1000000000);
    return sign ? ns : - ns;
}

/* +++++++++++++++++++++++++++++++ TIMINGS ++++++++++++++++++++++++++++++++++ */

RTIME rt_get_time(void)
{
    int cpuid;
    return rt_smp_oneshot_timer[cpuid = rtai_cpuid()] ? rdtsc() : rt_smp_times[cpuid].tick_time;
}

RTAI_SYSCALL_MODE RTIME rt_get_time_cpuid(unsigned int cpuid)
{
    return oneshot_timer ? rdtsc(): rt_times.tick_time;
}

RTIME rt_get_time_ns(void)
{
    int cpuid = rtai_cpuid();
    return oneshot_timer ? llimd(rdtsc(), 1000000000, tuned.cpu_freq) :
                       llimd(rt_times.tick_time, 1000000000, TIMER_FREQ);
}

RTAI_SYSCALL_MODE RTIME rt_get_time_ns_cpuid(unsigned int cpuid)
{
    return oneshot_timer ? llimd(rdtsc(), 1000000000, tuned.cpu_freq) :
                   llimd(rt_times.tick_time, 1000000000, TIMER_FREQ);
}

RTIME rt_get_cpu_time_ns(void)
{
    return llimd(rdtsc(), 1000000000, tuned.cpu_freq);
}

extern struct epoch_struct boot_epoch;

RTIME rt_get_real_time(void)
{
    return boot_epoch.time[boot_epoch.touse][0] + rtai_rdtsc();
}

RTIME rt_get_real_time_ns(void)
{
    return boot_epoch.time[boot_epoch.touse][1] + llimd(rtai_rdtsc(), 1000000000, tuned.cpu_freq);
}

/* +++++++++++++++++++++++++++ SECRET BACK DOORS ++++++++++++++++++++++++++++ */

RT_TASK *rt_get_base_linux_task(RT_TASK **base_linux_tasks)
{
        int cpuid;
        for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
                base_linux_tasks[cpuid] = rt_smp_linux_task + cpuid;
        }
        return rt_smp_linux_task;
}

RT_TASK *rt_alloc_dynamic_task(void)
{
#ifdef CONFIG_RTAI_MALLOC
        return rt_malloc(sizeof(RT_TASK)); // For VC's, proxies and C++ support.
#else
    return NULL;
#endif
}

/* +++++++++++++++++++++++++++ WATCHDOG SUPPORT ++++++++++++++++++++++++++++ */

RT_TASK **rt_register_watchdog(RT_TASK *wd, int cpuid)
{
        RT_TASK *task;

    if (lxrt_wdog_task[cpuid]) return (RT_TASK**) -EBUSY;
    task = &rt_linux_task;
    while ((task = task->next)) {
        if (task != wd && task->priority == RT_SCHED_HIGHEST_PRIORITY) {
            return (RT_TASK**) -EBUSY;
        }
    }
    lxrt_wdog_task[cpuid] = wd;
    return (RT_TASK**) 0;
}

void rt_deregister_watchdog(RT_TASK *wd, int cpuid)
{
        if (lxrt_wdog_task[cpuid] != wd) return;
    lxrt_wdog_task[cpuid] = NULL;
}

/* +++++++++++++++ SUPPORT FOR LINUX TASKS AND KERNEL THREADS +++++++++++++++ */

//#define ECHO_SYSW
#ifdef ECHO_SYSW
#define SYSW_DIAG_MSG(x) x
#else
#define SYSW_DIAG_MSG(x)
#endif

static RT_TRAP_HANDLER lxrt_old_trap_handler;

static inline void _rt_schedule_soft_tail(RT_TASK *rt_task, int cpuid)
{
    rt_global_cli();
    rt_task->state &= ~(RT_SCHED_READY | RT_SCHED_SFTRDY);
    (rt_task->rprev)->rnext = rt_task->rnext;
    (rt_task->rnext)->rprev = rt_task->rprev;
    rt_smp_current[cpuid] = &rt_linux_task;
    rt_schedule();
    UNLOCK_LINUX(cpuid);
    rt_global_sti();

#ifdef CONFIG_RTAI_ALIGN_LINUX_PRIORITY
do {
    int rt_priority;
    struct task_struct *lnxtsk;

    if ((lnxtsk = rt_task->lnxtsk)->policy == SCHED_FIFO || lnxtsk->policy == SCHED_RR) {
        if ((rt_priority = rt_task->priority) >= BASE_SOFT_PRIORITY) {
            rt_priority -= BASE_SOFT_PRIORITY;
        }
        if ((rt_priority = (MAX_LINUX_RTPRIO - rt_priority)) < 1) {
            rt_priority = 1;
        }
        if (rt_priority != lnxtsk->rt_priority) {
            rtai_set_linux_task_priority(lnxtsk, lnxtsk->policy, rt_priority);
        }
    }
} while (0);
#endif
}

void rt_schedule_soft(RT_TASK *rt_task)
{
    struct fun_args *funarg;
    int cpuid;

    rt_global_cli();
    rt_task->state |= RT_SCHED_READY;
    while (rt_task->state != RT_SCHED_READY) {
        current->state = TASK_SOFTREALTIME;
        rt_global_sti();
        schedule();
        rt_global_cli();
    }
    cpuid = rt_task->runnable_on_cpus;
    LOCK_LINUX(cpuid);
    enq_soft_ready_task(rt_task);
    rt_smp_current[cpuid] = rt_task;
    rt_global_sti();
    funarg = (void *)rt_task->fun_args;
    rt_task->retval = funarg->fun(RTAI_FUNARGS);
    _rt_schedule_soft_tail(rt_task, cpuid);
}

void rt_schedule_soft_tail(RT_TASK *rt_task, int cpuid)
{
    _rt_schedule_soft_tail(rt_task, cpuid);
}

static inline void fast_schedule(RT_TASK *new_task, struct task_struct *lnxtsk, int cpuid)
{
    RT_TASK *rt_current;
    rt_global_cli();
    new_task->state |= RT_SCHED_READY;
    enq_soft_ready_task(new_task);
    sched_release_global_lock(cpuid);
    LOCK_LINUX(cpuid);
    (rt_current = &rt_linux_task)->lnxtsk = lnxtsk;
    SET_EXEC_TIME();
    rt_smp_current[cpuid] = new_task;
    lxrt_context_switch(lnxtsk, new_task->lnxtsk, cpuid);
    CALL_TIMER_HANDLER();
    UNLOCK_LINUX(cpuid);
    rtai_sti();
}


static RT_TASK thread_task[NR_RT_CPUS];
static int rsvr_cnt[NR_RT_CPUS];

#if USE_RTAI_TASKS
#define RESERVOIR 0
#else
#define RESERVOIR 11
#endif
static int Reservoir = RESERVOIR;
RTAI_MODULE_PARM(Reservoir, int);
static int SpareKthreads = 100;
RTAI_MODULE_PARM(SpareKthreads, int);

static int taskidx[NR_RT_CPUS];
static struct task_struct **taskav[NR_RT_CPUS];

static struct task_struct *__get_kthread(int cpuid)
{
    unsigned long flags;
    struct task_struct *p;

    flags = rt_global_save_flags_and_cli();
    if (taskidx[cpuid] > 0) {
        p = taskav[cpuid][--taskidx[cpuid]];
        rt_global_restore_flags(flags);
        return p;
    }
    rt_global_restore_flags(flags);
    return 0;
}


/* detach the kernel thread from user space; not fully, only:
   session, process-group, tty. */ 

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)

void rt_daemonize(void)
{
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
    current->session = 1;
    current->pgrp    = 1;
    current->tty     = NULL;
    spin_lock_irq(&current->sigmask_lock);
    sigfillset(&current->blocked);
    recalc_sigpending(current);
    spin_unlock_irq(&current->sigmask_lock);
#else
#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,19)
    (current->signal)->__session = 1;
#else
    (current->signal)->session   = 1;
#endif
    (current->signal)->pgrp    = 1;
    (current->signal)->tty     = NULL;
#endif
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
    spin_lock_irq(&current->sigmask_lock);
    sigfillset(&current->blocked);
    recalc_sigpending(current);
    spin_unlock_irq(&current->sigmask_lock);
#else
    spin_lock_irq(&(current->sighand)->siglock);
    sigfillset(&current->blocked);
    recalc_sigpending();
    spin_unlock_irq(&(current->sighand)->siglock);
#endif
}

EXPORT_SYMBOL(rt_daemonize);

#else

extern void rt_daemonize(void);

#endif

#define HARD_KTHREAD_IN_USE ((char)220)

static void kthread_fun(int cpuid) 
{
    void steal_from_linux(RT_TASK *);
    void give_back_to_linux(RT_TASK *, int);
    RT_TASK *task;

#ifdef OOM_DISABLE
    current->oomkilladj = OOM_DISABLE;
#endif
    rt_daemonize();
    rtai_set_linux_task_priority(current, SCHED_FIFO, KTHREAD_F_PRIO);
    sprintf(current->comm, "F:HARD:%d:%d", cpuid, ++rsvr_cnt[cpuid]);
    current->rtai_tskext(TSKEXT0) = task = &thread_task[cpuid];
    current->rtai_tskext(TSKEXT1) = task->lnxtsk = current;
    put_current_on_cpu(cpuid);
    init_hard_fpu(current);
    task->msg_queue.next = &task->msg_queue;
    task->resq.next = &task->resq;
#ifdef PF_EVNOTIFY
    current->flags |= PF_EVNOTIFY;
#endif
    steal_from_linux(task);
    while(1) {
        rt_task_suspend(task);
        current->comm[0] = HARD_KTHREAD_IN_USE; //'U'
        if (!(task = current->rtai_tskext(TSKEXT0))->max_msg_size[0]) {
            break;
        }
#if CONFIG_RTAI_MONITOR_EXECTIME
        task->exectime[1] = rdtsc();
#endif
        ((void (*)(long))task->max_msg_size[0])(task->max_msg_size[1]);
        task->owndres = 0;
        current->comm[0] = 'F';
        current->rtai_tskext(TSKEXT1) = 0;
        rtai_cli();
        if (taskidx[cpuid] < SpareKthreads) {
            taskav[cpuid][taskidx[cpuid]++] = task->lnxtsk;
        }
        rtai_sti();
    }
    give_back_to_linux(task, 0);
    clr_rtext(task);
}

#define WAKE_UP_TASKs(klist) \
do { \
    struct klist_t *p = &klist[cpuid]; \
    struct task_struct *task; \
    while (p->out != p->in) { \
        task = p->task[p->out++ & (MAX_WAKEUP_SRQ - 1)]; \
        set_task_state(task, TASK_UNINTERRUPTIBLE); \
        wake_up_process(task); \
    } \
} while (0)

#define WAKE_UP_THREADM(thread) \
    do { \
        set_task_state(thread, TASK_UNINTERRUPTIBLE); \
        wake_up_process(thread); \
    } while (0)

static void kthread_m(int cpuid)
{
    struct task_struct *lnxtsk;
    struct klist_t *klistp;
    RT_TASK *task;
    
#ifdef OOM_DISABLE
    current->oomkilladj = OOM_DISABLE;
#endif
    rt_daemonize();
    (task = &thread_task[cpuid])->magic = RT_TASK_MAGIC;
    task->runnable_on_cpus = cpuid;
    sprintf(current->comm, "RTAI_KTHRD_M:%d", cpuid);
    put_current_on_cpu(cpuid);
    kthreadm[cpuid] = current;
    klistp = &klistm[cpuid];
    rtai_set_linux_task_priority(current, SCHED_FIFO, KTHREAD_M_PRIO);
    up(&resem[cpuid]);
    while (!endkthread) {
        current->state = TASK_RTAISRVSLEEP;
        schedule();
#if defined(CONFIG_SMP) && LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
        WAKE_UP_TASKs(wake_up_hts);
#endif
        while (klistp->out != klistp->in) {
            unsigned long hard, flags;
            flags = rt_global_save_flags_and_cli();
            hard = (unsigned long)(lnxtsk = klistp->task[klistp->out++ & (MAX_WAKEUP_SRQ - 1)]);
            if (hard > 1) {
                if (lnxtsk->rtai_tskext(TSKEXT2)) {
                    if (lnxtsk->rtai_tskext(TSKEXT1) && taskidx[cpuid] < SpareKthreads) {;
                        taskav[cpuid][taskidx[cpuid]++] = lnxtsk;
                        lnxtsk->comm[0] = 'F';
                    }
                    kthread_fun_long_jump(lnxtsk);
                }
            } else {
                if (taskidx[cpuid] < Reservoir) {
                    task->suspdepth = task->state = 0;
                    rt_global_sti();
                    kernel_thread((void *)kthread_fun, (void *)(long)cpuid, 0);
                    while (task->state != (RT_SCHED_READY | RT_SCHED_SUSPENDED)) {
                        current->state = TASK_INTERRUPTIBLE;
                        schedule_timeout(2);
                    }
                    kthread_fun_set_jump(task->lnxtsk);
                    rt_global_cli();
                    taskav[cpuid][taskidx[cpuid]++] = (void *)task->lnxtsk;
                }
                if (hard) {
                    rt_task_resume((void *)klistp->task[klistp->out++ & (MAX_WAKEUP_SRQ - 1)]);
                } else {
                    rt_global_sti();
                    up(&resem[cpuid]);
                    rt_global_cli();
                }
            }
            rt_global_restore_flags(flags);
        }
    }
    kthreadm[cpuid] = 0;
}

void steal_from_linux(RT_TASK *rt_task)
{
    struct klist_t *klistp;
    struct task_struct *lnxtsk;

    if (signal_pending(rt_task->lnxtsk)) {
        rt_task->is_hard = -1;
        return;
    }
    klistp = &wake_up_sth[rt_task->runnable_on_cpus];
    rtai_cli();
    klistp->task[klistp->in++ & (MAX_WAKEUP_SRQ - 1)] = rt_task;
    if (rt_task->base_priority >= BASE_SOFT_PRIORITY) {
        rt_task->base_priority -= BASE_SOFT_PRIORITY;
    }
    if (rt_task->priority >= BASE_SOFT_PRIORITY) {
        rt_task->priority -= BASE_SOFT_PRIORITY;
    }
    rt_task->is_hard = 1;
#if defined(TASK_ATOMICSWITCH) && TASK_ATOMICSWITCH && defined(CONFIG_PREEMPT)
    preempt_disable();
    (lnxtsk = rt_task->lnxtsk)->state = (TASK_HARDREALTIME | TASK_ATOMICSWITCH);
    rtai_sti();
#else
    (lnxtsk = rt_task->lnxtsk)->state = TASK_HARDREALTIME;
#endif
    do {
        schedule();
    } while (rt_task->state != RT_SCHED_READY);
#if CONFIG_RTAI_MONITOR_EXECTIME
    if (!rt_task->exectime[1]) {
        rt_task->exectime[1] = rdtsc();
    }
#endif
    if (lnxtsk_uses_fpu(lnxtsk)) {
        rtai_cli();
        restore_fpu(lnxtsk);
        rtai_sti();
    }
}

void give_back_to_linux(RT_TASK *rt_task, int keeprio)
{
    struct task_struct *lnxtsk;
    int rt_priority;

    rt_global_cli();
    (rt_task->rprev)->rnext = rt_task->rnext;
    (rt_task->rnext)->rprev = rt_task->rprev;
    rt_task->state = 0;
    pend_wake_up_hts(lnxtsk = rt_task->lnxtsk, rt_task->runnable_on_cpus);
#ifdef TASK_NOWAKEUP
    set_task_state(lnxtsk, lnxtsk->state & ~TASK_NOWAKEUP);
#endif
    rt_schedule();
    if (!(rt_task->is_hard = keeprio)) {
        if (rt_task->priority < BASE_SOFT_PRIORITY) {
            rt_priority = rt_task->priority;
            if (rt_task->priority == rt_task->base_priority) {
                rt_task->priority += BASE_SOFT_PRIORITY;
            }
        } else {
            rt_priority = rt_task->priority - BASE_SOFT_PRIORITY;
        }
        if (rt_task->base_priority < BASE_SOFT_PRIORITY) {
            rt_task->base_priority += BASE_SOFT_PRIORITY;
        }
    } else {
        if (rt_task->priority < BASE_SOFT_PRIORITY) {
            rt_priority = rt_task->priority;
        } else {
            rt_priority = rt_task->priority - BASE_SOFT_PRIORITY;
        }
    }
    rt_global_sti();
    /* Perform Linux's scheduling tail now since we woke up
       outside the regular schedule() point. */
    hal_schedule_back_root(lnxtsk);

#ifdef CONFIG_RTAI_ALIGN_LINUX_PRIORITY
    if (lnxtsk->policy == SCHED_FIFO || lnxtsk->policy == SCHED_RR) {
        if ((rt_priority = (MAX_LINUX_RTPRIO - rt_priority)) < 1) {
            rt_priority = 1;
        }
        if (rt_priority != lnxtsk->rt_priority) {
            rtai_set_linux_task_priority(lnxtsk, lnxtsk->policy, rt_priority);
        }
    }
#endif

    return;
}

static struct task_struct *get_kthread(int get, int cpuid, void *lnxtsk)
{
    struct task_struct *kthread;
    struct klist_t *klistp;
    RT_TASK *this_task;
    int hard;

    klistp = &klistm[cpuid];
    if (get) {
        while (!(kthread = __get_kthread(cpuid))) {
            this_task = rt_smp_current[cpuid];
            rt_global_cli();
            klistp->task[klistp->in++ & (MAX_WAKEUP_SRQ - 1)] = (void *)(long)(hard = this_task->is_hard > 0 ? 1 : 0);
            klistp->task[klistp->in++ & (MAX_WAKEUP_SRQ - 1)] = (void *)this_task;
            pend_wake_up_srq(kthreadm[cpuid], cpuid);
            rt_global_sti();
                if (hard) {
                rt_task_suspend(this_task);
            } else {
                down(&resem[cpuid]);
            }
        }
        rt_global_cli();
        klistp->task[klistp->in++ & (MAX_WAKEUP_SRQ - 1)] = 0;
        klistp->task[klistp->in++ & (MAX_WAKEUP_SRQ - 1)] = 0;
    } else {
        kthread = 0;
        rt_global_cli();
        klistp->task[klistp->in++ & (MAX_WAKEUP_SRQ - 1)] = lnxtsk;
    }
    pend_wake_up_srq(kthreadm[cpuid], cpuid);
    rt_global_sti();
    return kthread;
}

static void start_stop_kthread(RT_TASK *task, void (*rt_thread)(long), long data, int priority, int uses_fpu, void(*signal)(void), int runnable_on_cpus)
{
    if (num_online_cpus() == 1) {
        runnable_on_cpus = 0;
    }
    if (rt_thread) {
        task->retval = set_rtext(task, priority, uses_fpu, signal, runnable_on_cpus, get_kthread(1, runnable_on_cpus, 0));
        task->max_msg_size[0] = (long)rt_thread;
        task->max_msg_size[1] = data;
    } else {
        get_kthread(0, task->runnable_on_cpus, task->lnxtsk);
    }
}

static void wake_up_srq_handler(unsigned srq)
{
#ifdef CONFIG_SMP
    int cpuid = srq - wake_up_srq[0].srq;
#else
    int cpuid = 0; // optimises, ok for x86_64-2.6.10, which can be UP only.
#endif
#if !defined(CONFIG_SMP) || LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
    WAKE_UP_TASKs(wake_up_hts);
#else
    WAKE_UP_THREADM(kthreadm[cpuid]);
#endif
    WAKE_UP_TASKs(wake_up_srq);
    set_need_resched();
}

static unsigned long traptrans, systrans;

static int lxrt_handle_trap(int vec, int signo, struct pt_regs *regs, void *dummy_data)
{
    RT_TASK *rt_task;

    rt_task = rt_smp_current[rtai_cpuid()];
    if ((USE_RTAI_TASKS && !rt_task->lnxtsk) || (rt_task->lnxtsk)->comm[0] == HARD_KTHREAD_IN_USE) {
        if (rt_task->task_trap_handler[vec]) {
            return rt_task->task_trap_handler[vec](vec, signo, regs, rt_task);
        }
        rt_printk("Default Trap Handler: vector %d: Suspend RT task %p\n", vec, rt_task);
        rt_task_suspend(rt_task);
        return 1;
    }

    if (rt_task->is_hard > 0) {
        if (!traptrans++) {
            rt_printk("\nLXRT CHANGED MODE (TRAP), PID = %d, VEC = %d, SIGNO = %d.\n", (rt_task->lnxtsk)->pid, vec, signo);
        }
        SYSW_DIAG_MSG(rt_printk("\nFORCING IT SOFT (TRAP), PID = %d, VEC = %d, SIGNO = %d.\n", (rt_task->lnxtsk)->pid, vec, signo););
        give_back_to_linux(rt_task, -1);
        SYSW_DIAG_MSG(rt_printk("FORCED IT SOFT (TRAP), PID = %d, VEC = %d, SIGNO = %d.\n", (rt_task->lnxtsk)->pid, vec, signo););
    }

    return 0;
}

static inline void rt_signal_wake_up(RT_TASK *task)
{
    if (task->state && task->state != RT_SCHED_READY) {
#ifdef TASK_NOWAKEUP
        struct task_struct *lnxtsk;
        if ((lnxtsk = task->lnxtsk)->state & TASK_HARDREALTIME) {
            set_task_state(lnxtsk, lnxtsk->state | TASK_NOWAKEUP);
        }
#endif
        task->unblocked = 1;
        rt_task_masked_unblock(task, ~RT_SCHED_READY);
    } else {
        task->unblocked = -1;
    }
}

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,32)

#define INTERCEPT_SCHEDULE_HEAD() \
    rtai_catch_event(hal_root_domain, HAL_SCHEDULE_HEAD, (void *)lxrt_intercept_schedule_head)

#define MAX_MM2DROP 32  /* must be a power of 2. */
static struct mmreq {
    unsigned long in, out, count;
    struct mm_struct *mm[MAX_MM2DROP];
} mm2drop_tab[NR_CPUS];

struct prev_next_t { struct task_struct *prev, *next; };

static int lxrt_intercept_schedule_head (unsigned long event, struct prev_next_t *evdata)
{
    struct task_struct *prev = evdata->prev;

    if (!prev->mm) {
        struct mmreq *p = mm2drop_tab + prev->processor;
        struct mm_struct *oldmm = prev->active_mm;
        BUG_ON(p->count >= MAX_MM2DROP);
        atomic_inc(&oldmm->mm_count);
        p->mm[p->in++ & (MAX_MM2DROP - 1)] = oldmm;
        p->count++;
    }

    return 0;
}

#define DROP_MM2DROP(cpuid) \
    do { \
        struct mmreq *p = mm2drop_tab + cpuid; \
        while (p->out != p->in) { \
            struct mm_struct *oldmm = p->mm[p->out++ & (MAX_MM2DROP - 1)]; \
            mmdrop(oldmm); \
            p->count--; \
        } \
    } while (0)

#define RELEASE_SCHEDULE_HEAD() \
    rtai_catch_event(hal_root_domain, HAL_SCHEDULE_HEAD, NULL)

#define DROP_ALL_PENDING_MM2DROP() \
    do { \
        unsigned long flags, cpuid; \
        flags = rtai_critical_enter(NULL); \
        for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) { \
            DROP_MM2DROP(cpuid); \
        } \
        rtai_critical_exit(flags); \
    } while (0)

#else  /* KERNEL_VERSION >= 2.4.32 */

#define INTERCEPT_SCHEDULE_HEAD()   do { } while (0)

#define DROP_MM2DROP(cpuid)         do { } while (0)

#define RELEASE_SCHEDULE_HEAD()     do { } while (0)

#define DROP_ALL_PENDING_MM2DROP()  do { } while (0)

#endif  /* KERNEL_VERSION < 2.4.32 */

static int lxrt_intercept_schedule_tail (unsigned event, void *nothing)

{
    int cpuid = rtai_cpuid();
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
    if (in_hrt_mode(cpuid)) {
        return 1;
    } else 
#endif
    {
        struct klist_t *klistp = &wake_up_sth[cpuid];
        while (klistp->out != klistp->in) {
            fast_schedule(klistp->task[klistp->out++ & (MAX_WAKEUP_SRQ - 1)], current, cpuid);
        }
    }

    DROP_MM2DROP(cpuid);

    return 0;
}

struct sig_wakeup_t { struct task_struct *task; };
static int lxrt_intercept_sig_wakeup (long event, void *data)
{
    RT_TASK *task;
    if ((task = INTERCEPT_WAKE_UP_TASK(data)->rtai_tskext(TSKEXT0))) {
        rt_signal_wake_up(task);
        return 1;
    }
    return 0;
}

static int lxrt_intercept_exit (unsigned long event, struct task_struct *lnx_task)
{
    extern void linux_process_termination(void);
    RT_TASK *task;
    if ((task = lnx_task->rtai_tskext(TSKEXT0))) {
        if (task->is_hard > 0) {
            give_back_to_linux(task, 0);
        }
        linux_process_termination();
    }
    return 0;
}

extern long long rtai_lxrt_invoke (unsigned long, void *, void *);
extern int (*sys_call_table[])(struct pt_regs);

#if 0
static RT_TASK *server_task_init(int prio, int cpus_allowed)
{
    RT_TASK *tsk;
    if ((tsk = rt_malloc(sizeof(RT_TASK) + 2*sizeof(struct fun_args)))) { 
        tsk->magic = 0;
        if (!set_rtext(tsk, prio, 0, 0, cpus_allowed, 0)) {
            tsk->fun_args = (long *)((struct fun_args *)(tsk + 1));
            if (rt_register((unsigned long)tsk, tsk, IS_TASK, 0)) {
                return tsk;
            } else {
                clr_rtext(tsk);
            }
        }
        rt_free(tsk);
    }
    return 0;
}

static inline RT_TASK *soft_rt_linux_server_call(RT_TASK *task, void *fun, void *arg1, void *arg2)
{
    task->fun_args[0] = (long)arg1;
    task->fun_args[1] = (long)arg2;
    ((struct fun_args *)task->fun_args)->fun = fun;
    rt_schedule_soft(task);
    return (RT_TASK *)(unsigned long)task->retval;
}

static void linux_syscall_server_fun(RT_TASK *master_task)
{
    RT_TASK *server_task;
        struct pt_regs regs;

    master_task->linux_syscall_server = server_task = server_task_init(master_task->base_priority >= BASE_SOFT_PRIORITY ? master_task->base_priority - BASE_SOFT_PRIORITY : master_task->base_priority, master_task->runnable_on_cpus);
    rt_task_resume(master_task);
    while (soft_rt_linux_server_call(server_task, rt_receive_linux_syscall, master_task, &regs) == master_task) {
        rt_return_linux_syscall(master_task, sys_call_table[regs.LINUX_SYSCALL_NR](regs));
    }
}

RT_TASK *lxrt_init_linux_server(RT_TASK *master_task)
{
    int is_hard;
    if (!master_task) {
        if (!current->rtai_tskext(TSKEXT0)) {
            return NULL;
        }
        master_task = current->rtai_tskext(TSKEXT0);
    }
    if (!master_task->lnxtsk) {
        return NULL;
    }
    if ((is_hard = master_task->is_hard) > 0) {
        give_back_to_linux(master_task, 0);
    }
    master_task->linux_syscall_server = NULL;
    kernel_thread((void *)linux_syscall_server_fun, master_task, CLONE_VM | CLONE_FS | CLONE_FILES | CLONE_SIGHAND);
    soft_rt_linux_server_call(master_task, rt_task_suspend, master_task, NULL);
    if (is_hard > 0) {
        steal_from_linux(master_task);
    }
    return master_task->linux_syscall_server;
}

#endif

static int lxrt_intercept_syscall_prologue(struct pt_regs *regs)
{
    RT_TASK *task;

    if (regs->LINUX_SYSCALL_NR < NR_syscalls && (task = current->rtai_tskext(TSKEXT0))) {
        if (task->is_hard > 0) {
            if (task->linux_syscall_server) {
                rt_exec_linux_syscall(task, task->linux_syscall_server, regs);
                return 1;
            }
            if (!systrans++) {
                rt_printk("\nLXRT CHANGED MODE (SYSCALL), PID = %d, SYSCALL = %lu.\n", (task->lnxtsk)->pid, regs->LINUX_SYSCALL_NR);
            }
            SYSW_DIAG_MSG(rt_printk("\nFORCING IT SOFT (SYSCALL), PID = %d, SYSCALL = %d.\n", (task->lnxtsk)->pid, regs->LINUX_SYSCALL_NR););
            give_back_to_linux(task, -1);
            SKIP_IMMEDIATE_LINUX_SYSCALL();
            if (signal_pending(task->lnxtsk)) return 0;
            SYSW_DIAG_MSG(rt_printk("FORCED IT SOFT, CALLING LINUX (SYSCALL), PID = %d, SYSCALL = %d.\n", (task->lnxtsk)->pid, regs->LINUX_SYSCALL_NR););
            LXRT_DO_IMMEDIATE_LINUX_SYSCALL(regs);
            SYSW_DIAG_MSG(rt_printk("LINUX RETURNED, GOING BACK TO HARD (SYSLXRT), PID = %d.\n", current->pid););
            steal_from_linux(task);
            SYSW_DIAG_MSG(rt_printk("GONE BACK TO HARD (SYSLXRT),  PID = %d.\n", current->pid););
            return 1;
        }
    }
    return 0;
}

static int lxrt_intercept_syscall_epilogue(unsigned long event, void *nothing)
{
    RT_TASK *task;
    if ((task = (RT_TASK *)current->rtai_tskext(TSKEXT0))) {
        if (task->system_data_ptr) {
            struct pt_regs *r = task->system_data_ptr;
            r->LINUX_SYSCALL_RETREG = -ERESTARTSYS;
            r->LINUX_SYSCALL_NR = RTAI_SYSCALL_NR;
            task->system_data_ptr = NULL;
        } else if (task->is_hard < 0) {
            SYSW_DIAG_MSG(rt_printk("GOING BACK TO HARD (SYSLXRT), PID = %d.\n", current->pid););
            steal_from_linux(task);
            SYSW_DIAG_MSG(rt_printk("GONE BACK TO HARD (SYSLXRT),  PID = %d.\n", current->pid););
            return 1;
        }
    }
    return 0;
}

/* ++++++++++++++++++++++++++ SCHEDULER PROC FILE +++++++++++++++++++++++++++ */

#ifdef CONFIG_PROC_FS
/* -----------------------< proc filesystem section >-------------------------*/

extern int rtai_global_heap_size;

#ifdef CONFIG_RTAI_USE_TLSF
#define RTAI_USES_TLSF  1
extern unsigned long tlsf_get_used_size(rtheap_t *);
#define rt_get_heap_mem_used(heap)  tlsf_get_used_size(heap)
#else
#define RTAI_USES_TLSF  0
#define rt_get_heap_mem_used(heap)  rtheap_used_mem(heap)
#endif

static int rtai_read_sched(char *page, char **start, off_t off, int count,
                           int *eof, void *data)
{
    PROC_PRINT_VARS;
        int cpuid, i = 1;
    unsigned long t;
    RT_TASK *task;

    PROC_PRINT("\nRTAI LXRT Real Time Task Scheduler.\n\n");
    PROC_PRINT("    Calibrated Time Base Frequency: %lu Hz\n", tuned.cpu_freq);
    PROC_PRINT("    Calibrated interrupt to scheduler latency: %d ns\n", (int)imuldiv(tuned.latency - tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq));
    PROC_PRINT("    Calibrated oneshot timer setup_to_firing time: %d ns\n\n",
                  (int)imuldiv(tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq));
    PROC_PRINT("Number of RT CPUs in system: %d (sized for %d)\n\n", num_online_cpus(), NR_RT_CPUS);
 
    PROC_PRINT("Real time kthreads in resorvoir (cpu/#)");
        for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
                PROC_PRINT(": (%d/%d)", cpuid, taskidx[cpuid]);
        }
    PROC_PRINT("\n\n");

    PROC_PRINT("Global heap: size = %10d, used = %10lu; <%s>.\n", rtai_global_heap_size, rt_get_heap_mem_used(&rtai_global_heap), RTAI_USES_TLSF ? "TLSF" : "BSD");

    PROC_PRINT("Kstack heap: size = %10d, used = %10lu; <%s>.\n\n", rtai_kstack_heap_size, rt_get_heap_mem_used(&rtai_kstack_heap), RTAI_USES_TLSF ? "TLSF" : "BSD");

    PROC_PRINT("Number of forced hard/soft/hard transitions: traps %lu, syscalls %lu\n\n", traptrans, systrans);

    PROC_PRINT("Priority  Period(ns)  FPU  Sig  State  CPU  Task  HD/SF  PID  RT_TASK *  TIME\n" );
    PROC_PRINT("------------------------------------------------------------------------------\n" );
        for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
                task = &rt_linux_task;
/*
* Display all the active RT tasks and their state.
*
* Note: As a temporary hack the tasks are given an id which is
*       the order they appear in the task list, needs fixing!
*/
        while ((task = task->next)) {
/*
* The display for the task period is set to an integer (%d) as 64 bit
* numbers are not currently handled correctly by the kernel routines.
* Hence the period display will be wrong for time periods > ~4 secs.
*/
            t = 0;
            if ((!task->lnxtsk || task->is_hard) && task->exectime[1]) {
                unsigned long den = (unsigned long)llimd(rdtsc() - task->exectime[1], 10, tuned.cpu_freq);
                if (den) {
                    t = 1000UL*(unsigned long)llimd(task->exectime[0], 10, tuned.cpu_freq)/den;
                }               
            }
            PROC_PRINT("%-10d %-11lu %-4s %-3s 0x%-3x  %1lu:%1lu   %-4d   %-4d %-4d  %p   %-lu\n",
                               task->priority,
                               (unsigned long)count2nano_cpuid(task->period, task->runnable_on_cpus),
                               task->uses_fpu || task->lnxtsk ? "Yes" : "No",
                               task->signal ? "Yes" : "No",
                               task->state,
                   task->runnable_on_cpus, // cpuid,
                   task->lnxtsk ? CPUMASK((task->lnxtsk)->cpus_allowed) : (1 << task->runnable_on_cpus),
                               i,
                   task->is_hard,
                   task->lnxtsk ? task->lnxtsk->pid : 0,
                   task, t);
            i++;
                } /* End while loop - display all RT tasks on a CPU. */

        PROC_PRINT("TIMED\n");
        task = &rt_linux_task;
        while ((task = task->tnext) != &rt_linux_task) {
            PROC_PRINT("> %p ", task);
        }
        PROC_PRINT("\nREADY\n");
        task = &rt_linux_task;
        while ((task = task->rnext) != &rt_linux_task) {
            PROC_PRINT("> %p ", task);
        }

        }  /* End for loop - display RT tasks on all CPUs. */

    PROC_PRINT_DONE;

}  /* End function - rtai_read_sched */


static int rtai_proc_sched_register(void) 
{
        struct proc_dir_entry *proc_sched_ent;


        proc_sched_ent = create_proc_entry("scheduler", S_IFREG|S_IRUGO|S_IWUSR, rtai_proc_root);
        if (!proc_sched_ent) {
                printk("Unable to initialize /proc/rtai/scheduler\n");
                return(-1);
        }
        proc_sched_ent->read_proc = rtai_read_sched;
        return(0);
}  /* End function - rtai_proc_sched_register */


static void rtai_proc_sched_unregister(void) 
{
        remove_proc_entry("scheduler", rtai_proc_root);
}  /* End function - rtai_proc_sched_unregister */

/* --------------------< end of proc filesystem section >---------------------*/
#endif /* CONFIG_PROC_FS */

/* ++++++++++++++ SCHEDULER ENTRIES AND RELATED INITIALISATION ++++++++++++++ */

extern void usp_request_rtc(int, void *);
extern void rt_release_rtc(void);

static int rt_gettid(void)
{
    return current->pid;
}

static struct rt_native_fun_entry rt_sched_entries[] = {
    { { 0, rt_set_runnable_on_cpus },       SET_RUNNABLE_ON_CPUS },
    { { 0, rt_set_runnable_on_cpuid },      SET_RUNNABLE_ON_CPUID },
    { { 0, rt_set_sched_policy },           SET_SCHED_POLICY },
    { { 0, rt_get_timer_cpu },          GET_TIMER_CPU },
    { { 0, rt_is_hard_timer_running },      HARD_TIMER_RUNNING },
    { { 0, rt_set_periodic_mode },          SET_PERIODIC_MODE },
    { { 0, rt_set_oneshot_mode },           SET_ONESHOT_MODE },
    { { 0, start_rt_timer },            START_TIMER },
    { { 0, start_rt_apic_timers },          START_RT_APIC_TIMERS },
    { { 0, stop_rt_timer },             STOP_TIMER },
    { { 0, rt_task_signal_handler },        SIGNAL_HANDLER  },
    { { 0, rt_task_use_fpu },           TASK_USE_FPU },
    { { 0, rt_hard_timer_tick_count },      HARD_TIMER_COUNT },
    { { 0, rt_hard_timer_tick_count_cpuid },    HARD_TIMER_COUNT_CPUID },
    { { 0, count2nano },                COUNT2NANO },
    { { 0, nano2count },                NANO2COUNT },
    { { 0, count2nano_cpuid },          COUNT2NANO_CPUID },
    { { 0, nano2count_cpuid },          NANO2COUNT_CPUID },
    { { 0, rt_get_time },               GET_TIME },
    { { 0, rt_get_time_cpuid },         GET_TIME_CPUID },
    { { 0, rt_get_time_ns },            GET_TIME_NS },
    { { 0, rt_get_time_ns_cpuid },          GET_TIME_NS_CPUID },
    { { 0, rt_get_cpu_time_ns },            GET_CPU_TIME_NS },
    { { 0, rt_task_get_info },          GET_TASK_INFO },
    { { 0, rt_spv_RMS },                SPV_RMS },
    { { 1, rt_change_prio },            CHANGE_TASK_PRIO },
    { { 0, rt_sched_lock },             SCHED_LOCK },
    { { 0, rt_sched_unlock },           SCHED_UNLOCK },
    { { 1, rt_task_yield },             YIELD },  
    { { 1, rt_task_suspend },           SUSPEND },
    { { 1, rt_task_suspend_if },            SUSPEND_IF },
    { { 1, rt_task_suspend_until },         SUSPEND_UNTIL },
    { { 1, rt_task_suspend_timed },         SUSPEND_TIMED },
    { { 1, rt_task_resume },            RESUME },
    { { 1, rt_set_linux_syscall_mode },     SET_LINUX_SYSCALL_MODE },
#ifdef CONFIG_RTAI_USI
    { { 1, rt_irq_wait },               IRQ_WAIT },
    { { 1, rt_irq_wait_if },            IRQ_WAIT_IF },
    { { 1, rt_irq_wait_until },         IRQ_WAIT_UNTIL },
    { { 1, rt_irq_wait_timed },         IRQ_WAIT_TIMED },
    { { 0, rt_irq_signal },             IRQ_SIGNAL },
    { { 0, rt_request_irq_task },           REQUEST_IRQ_TASK },
    { { 0, rt_release_irq_task },           RELEASE_IRQ_TASK },
    { { 0, usp_request_rtc },                   REQUEST_RTC },
#endif
    { { 1, rt_task_make_periodic_relative_ns }, MAKE_PERIODIC_NS },
    { { 1, rt_task_make_periodic },         MAKE_PERIODIC },
    { { 1, rt_task_set_resume_end_times },      SET_RESUME_END },
    { { 0, rt_set_resume_time },            SET_RESUME_TIME },
    { { 0, rt_set_period },             SET_PERIOD },
    { { 1, rt_task_wait_period },           WAIT_PERIOD },
    { { 0, rt_busy_sleep },             BUSY_SLEEP },
    { { 1, rt_sleep },              SLEEP },
    { { 1, rt_sleep_until },            SLEEP_UNTIL },
    { { 0, rt_task_masked_unblock },        WAKEUP_SLEEPING },
    { { 0, rt_named_task_init },            NAMED_TASK_INIT },  
    { { 0, rt_named_task_init_cpuid },      NAMED_TASK_INIT_CPUID },  
    { { 0, rt_named_task_delete },          NAMED_TASK_DELETE },  
    { { 0, rt_get_name },               GET_NAME },
    { { 0, rt_get_adr },                GET_ADR },
    { { 0, usr_rt_pend_linux_irq },         PEND_LINUX_IRQ },
    { { 0, rt_release_rtc },                    RELEASE_RTC },
    { { 0, rt_gettid },                         RT_GETTID },
    { { 0, rt_get_real_time },          GET_REAL_TIME },
    { { 0, rt_get_real_time_ns },           GET_REAL_TIME_NS },
    { { 1, rt_signal_helper },          RT_SIGNAL_HELPER },
    { { 1, rt_wait_signal },            RT_SIGNAL_WAITSIG },
    { { 1, rt_request_signal_ },            RT_SIGNAL_REQUEST },
    { { 1, rt_release_signal },         RT_SIGNAL_RELEASE },
    { { 1, rt_enable_signal },          RT_SIGNAL_ENABLE },
    { { 1, rt_disable_signal },         RT_SIGNAL_DISABLE },
    { { 1, rt_trigger_signal },             RT_SIGNAL_TRIGGER },
    { { 0, 0 },                     000 }
};

extern void *rtai_lxrt_dispatcher;

DECLARE_FUSION_WAKE_UP_STUFF;

static int lxrt_init(void)

{
    void init_fun_ext(void);
    int cpuid;

    init_fun_ext();

    REQUEST_RESUME_SRQs_STUFF();

    /* We will start stealing Linux tasks as soon as the reservoir is
       instantiated, so create the migration service now.             */

    if (Reservoir <= 0) {
        Reservoir = 1;
    }

    Reservoir = (Reservoir + num_online_cpus() - 1)/num_online_cpus();

    for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
        taskav[cpuid] = (void *)kmalloc(SpareKthreads*sizeof(void *), GFP_KERNEL);
        init_MUTEX_LOCKED(&resem[cpuid]);
        kernel_thread((void *)kthread_m, (void *)(long)cpuid, 0);
        down(&resem[cpuid]);
        klistm[cpuid].in = (2*Reservoir) & (MAX_WAKEUP_SRQ - 1);
        WAKE_UP_THREADM(kthreadm[cpuid]);
    }

    for (cpuid = 0; cpuid < MAX_LXRT_FUN; cpuid++) {
        rt_fun_lxrt[cpuid].type = 1;
        rt_fun_lxrt[cpuid].fun  = nihil;
    }
    
    set_rt_fun_entries(rt_sched_entries);

    lxrt_old_trap_handler = rt_set_rtai_trap_handler(lxrt_handle_trap);

#ifdef CONFIG_PROC_FS
    rtai_proc_lxrt_register();
#endif

    INTERCEPT_SCHEDULE_HEAD();
    rtai_catch_event(hal_root_domain, HAL_SCHEDULE_TAIL, (void *)lxrt_intercept_schedule_tail);
    rtai_catch_event(hal_root_domain, HAL_SYSCALL_PROLOGUE, (void *)lxrt_intercept_syscall_prologue);
    rtai_catch_event(hal_root_domain, HAL_SYSCALL_EPILOGUE, (void *)lxrt_intercept_syscall_epilogue);
    rtai_catch_event(hal_root_domain, HAL_EXIT_PROCESS, (void *)lxrt_intercept_exit);
    rtai_catch_event(hal_root_domain, HAL_KICK_PROCESS, (void *)lxrt_intercept_sig_wakeup);
    rtai_lxrt_dispatcher = rtai_lxrt_invoke;

    return 0;
}

static void lxrt_exit(void)
{
    RT_TASK *rt_task;
    struct task_struct *kthread;
    int cpuid;

#ifdef CONFIG_PROC_FS
    rtai_proc_lxrt_unregister();
#endif

    rt_task = kmalloc(sizeof(RT_TASK), GFP_KERNEL);
    for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
        while ((kthread = __get_kthread(cpuid))) {
            if (kthread->rtai_tskext(TSKEXT2)) {
                kfree(kthread->rtai_tskext(TSKEXT2));
            }
            rt_task->magic = 0;
            set_rtext(rt_task, 0, 0, 0, cpuid, kthread);
            rt_task->max_msg_size[0] = 0;
            rt_task_resume(rt_task);
            while (rt_task->magic || rt_task->state) {
                current->state = TASK_INTERRUPTIBLE;
                schedule_timeout(2);
            }
        }
    }
    kfree(rt_task);

    endkthread = 1;
    for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
        WAKE_UP_THREADM(kthreadm[cpuid]);
        while (kthreadm[cpuid]) {
            current->state = TASK_INTERRUPTIBLE;
            schedule_timeout(2);
        }
        kfree(taskav[cpuid]);
    }

    rt_set_rtai_trap_handler(lxrt_old_trap_handler);

    RELEASE_RESUME_SRQs_STUFF();

    RELEASE_SCHEDULE_HEAD();
    rtai_catch_event(hal_root_domain, HAL_SCHEDULE_TAIL, NULL);
    rtai_catch_event(hal_root_domain, HAL_SYSCALL_PROLOGUE, NULL);
    rtai_catch_event(hal_root_domain, HAL_SYSCALL_EPILOGUE, NULL);
    rtai_catch_event(hal_root_domain, HAL_EXIT_PROCESS, NULL);
    rtai_catch_event(hal_root_domain, HAL_KICK_PROCESS, NULL);
    rtai_lxrt_dispatcher = NULL;
    
    DROP_ALL_PENDING_MM2DROP();

    reset_rt_fun_entries(rt_sched_entries);
}

#ifdef DECLR_8254_TSC_EMULATION
DECLR_8254_TSC_EMULATION;

static void timer_fun(unsigned long none)
{
    TICK_8254_TSC_EMULATION();
    timer.expires = jiffies + (HZ + TSC_EMULATION_GUARD_FREQ/2 - 1)/TSC_EMULATION_GUARD_FREQ;
    add_timer(&timer);
}
#endif

extern int rt_registry_alloc(void);
extern void rt_registry_free(void);

static int __rtai_lxrt_init(void)
{
    int cpuid, retval;
    
#ifdef IPIPE_NOSTACK_FLAG
//  ipipe_set_foreign_stack(&rtai_domain);
#endif

#ifdef CONFIG_RTAI_MALLOC
    rtai_kstack_heap_size = (rtai_kstack_heap_size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1);
    if (rtheap_init(&rtai_kstack_heap, NULL, rtai_kstack_heap_size, PAGE_SIZE, GFP_KERNEL)) {
        printk(KERN_INFO "RTAI[malloc]: failed to initialize the kernel stacks heap (size=%d bytes).\n", rtai_kstack_heap_size);
        return 1;
    }
#endif
    sched_mem_init();

    rt_registry_alloc();

    for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
        rt_linux_task.uses_fpu = 1;
        rt_linux_task.magic = 0;
        rt_linux_task.policy = rt_linux_task.is_hard = 0;
        rt_linux_task.runnable_on_cpus = cpuid;
        rt_linux_task.state = RT_SCHED_READY;
        rt_linux_task.msg_queue.prev = &(rt_linux_task.msg_queue);      
        rt_linux_task.msg_queue.next = &(rt_linux_task.msg_queue);      
        rt_linux_task.msg_queue.task = &rt_linux_task;    
        rt_linux_task.msg = 0;  
        rt_linux_task.ret_queue.prev = &(rt_linux_task.ret_queue);
        rt_linux_task.ret_queue.next = &(rt_linux_task.ret_queue);
        rt_linux_task.ret_queue.task = NULL;
        rt_linux_task.priority = RT_SCHED_LINUX_PRIORITY;
        rt_linux_task.base_priority = RT_SCHED_LINUX_PRIORITY;
        rt_linux_task.signal = 0;
        rt_linux_task.prev = &rt_linux_task;
                rt_linux_task.resume_time = RT_TIME_END;
                rt_linux_task.periodic_resume_time = RT_TIME_END;
                rt_linux_task.tprev = rt_linux_task.tnext =
                rt_linux_task.rprev = rt_linux_task.rnext = &rt_linux_task;
#ifdef CONFIG_RTAI_LONG_TIMED_LIST
        rt_linux_task.rbr.rb_node = NULL;
#endif
        rt_linux_task.next = 0;
        rt_linux_task.lnxtsk = current;
        rt_smp_current[cpuid] = &rt_linux_task;
        rt_smp_fpu_task[cpuid] = &rt_linux_task;
        oneshot_timer = OneShot ? 1 : 0;
        oneshot_running = 0;
        linux_cr0 = 0;
        rt_linux_task.resq.prev = rt_linux_task.resq.next = &rt_linux_task.resq;
        rt_linux_task.resq.task = NULL;
    }
    tuned.latency = imuldiv(Latency, tuned.cpu_freq, 1000000000);
    tuned.setup_time_TIMER_CPUNIT = imuldiv( SetupTimeTIMER, 
                         tuned.cpu_freq, 
                         1000000000);
    tuned.setup_time_TIMER_UNIT   = imuldiv( SetupTimeTIMER, 
                         TIMER_FREQ, 
                         1000000000);
    tuned.timers_tol[0] = 0;
    oneshot_span = ONESHOT_SPAN;
    satdlay = oneshot_span - tuned.latency;
#ifdef CONFIG_PROC_FS
    if (rtai_proc_sched_register()) {
        retval = 1;
        goto mem_end;
    }
#endif

// 0x7dd763ad == nam2num("MEMSRQ").
    if ((frstk_srq.srq = rt_request_srq(0x7dd763ad, frstk_srq_handler, 0)) < 0) {
        printk("MEM SRQ: no sysrq available.\n");
        retval = frstk_srq.srq;
        goto proc_unregister;
    }

    frstk_srq.in = frstk_srq.out = 0;
    if ((retval = rt_request_sched_ipi()) != 0)
        goto free_srq;

    if ((retval = lxrt_init()) != 0)
        goto free_sched_ipi;

#ifdef CONFIG_RTAI_SCHED_ISR_LOCK
    rt_set_ihook(&rtai_handle_isched_lock);
#endif /* CONFIG_RTAI_SCHED_ISR_LOCK */

    register_reboot_notifier(&lxrt_reboot_notifier);
#ifdef CONFIG_SMP
    printk(KERN_INFO "RTAI[sched]: IMMEDIATE, MP, USER/KERNEL SPACE: <%s RTAI OWN KTASKs>", USE_RTAI_TASKS ? "with" : "without");
#else
    printk(KERN_INFO "RTAI[sched]: loaded (IMMEDIATE, UP, USER/KERNEL SPACE: <%s RTAI OWN KTASKs>", USE_RTAI_TASKS ? "with" : "without");
#endif
#ifdef CONFIG_RTAI_LXRT_USE_LINUX_SYSCALL
    printk(", <uses LINUX SYSCALLs>");
#endif
#ifdef CONFIG_RTAI_MALLOC
    printk(", kstacks pool size = %d bytes", rtai_kstack_heap_size);
#endif
    printk(".\n");
    printk(KERN_INFO "RTAI[sched]: hard timer type/freq = %s/%d(Hz); default timing: %s; ", TIMER_NAME, (int)TIMER_FREQ, OneShot ? "oneshot" : "periodic");
#ifdef CONFIG_RTAI_LONG_TIMED_LIST
    printk("black/red timed lists.\n");
#else
    printk("linear timed lists.\n");
#endif
    printk(KERN_INFO "RTAI[sched]: Linux timer freq = %d (Hz), TimeBase freq = %lu hz.\n", HZ, (unsigned long)tuned.cpu_freq);
    printk(KERN_INFO "RTAI[sched]: timer setup = %d ns, resched latency = %d ns.\n", (int)imuldiv(tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq), (int)imuldiv(tuned.latency - tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq));

#ifdef DECLR_8254_TSC_EMULATION
    SETUP_8254_TSC_EMULATION;
#endif

    retval = rtai_init_features(); /* see rtai_schedcore.h */

exit:
#if defined(CONFIG_GENERIC_CLOCKEVENTS) && CONFIG_RTAI_RTC_FREQ == 0
    rt_linux_hrt_set_mode  = _rt_linux_hrt_set_mode;
    rt_linux_hrt_next_shot = _rt_linux_hrt_next_shot;
#endif
    return retval;
free_sched_ipi:
    rt_free_sched_ipi();
free_srq:
    rt_free_srq(frstk_srq.srq);
proc_unregister:
#ifdef CONFIG_PROC_FS
    rtai_proc_sched_unregister();
#endif
mem_end:
    sched_mem_end();
#ifdef CONFIG_RTAI_MALLOC
    rtheap_destroy(&rtai_kstack_heap, GFP_KERNEL);
#endif
    rt_registry_free();
    goto exit;
}

static void __rtai_lxrt_exit(void)
{
    unregister_reboot_notifier(&lxrt_reboot_notifier);

#if defined(CONFIG_GENERIC_CLOCKEVENTS) && CONFIG_RTAI_RTC_FREQ == 0
    rt_linux_hrt_set_mode  = NULL;
    rt_linux_hrt_next_shot = NULL;
#endif

    lxrt_killall();

    krtai_objects_release();

    lxrt_exit();

    rtai_cleanup_features();

#ifdef CONFIG_PROC_FS
        rtai_proc_sched_unregister();
#endif
    while (frstk_srq.out != frstk_srq.in);
    if (rt_free_srq(frstk_srq.srq) < 0) {
        printk("MEM SRQ: frstk_srq %d illegal or already free.\n", frstk_srq.srq);
    }
    rt_free_sched_ipi();
    sched_mem_end();
#ifdef CONFIG_RTAI_MALLOC
    rtheap_destroy(&rtai_kstack_heap, GFP_KERNEL);
#endif
    rt_registry_free();
    current->state = TASK_INTERRUPTIBLE;
    schedule_timeout(HZ/10);
#ifdef CONFIG_RTAI_SCHED_ISR_LOCK
    rt_set_ihook(NULL);
#endif /* CONFIG_RTAI_SCHED_ISR_LOCK */

#ifdef DECLR_8254_TSC_EMULATION
    CLEAR_8254_TSC_EMULATION;
#endif

#ifdef IPIPE_NOSTACK_FLAG
    ipipe_clear_foreign_stack(&rtai_domain);
#endif

    printk(KERN_INFO "RTAI[sched]: unloaded (forced hard/soft/hard transitions: traps %lu, syscalls %lu).\n", traptrans, systrans);
}

module_init(__rtai_lxrt_init);
module_exit(__rtai_lxrt_exit);

#ifndef CONFIG_KBUILD
#define CONFIG_KBUILD
#endif

#ifdef CONFIG_KBUILD

EXPORT_SYMBOL(rt_fun_lxrt);
EXPORT_SYMBOL(clr_rtext);
EXPORT_SYMBOL(set_rtext);
EXPORT_SYMBOL(get_min_tasks_cpuid);
EXPORT_SYMBOL(put_current_on_cpu);
EXPORT_SYMBOL(rt_schedule_soft);
EXPORT_SYMBOL(rt_do_force_soft);
EXPORT_SYMBOL(rt_schedule_soft_tail);
EXPORT_SYMBOL(rt_sched_timed);
EXPORT_SYMBOL(rtai_handle_isched_lock);
#if CONFIG_RTAI_MONITOR_EXECTIME
EXPORT_SYMBOL(switch_time);
#endif
EXPORT_SYMBOL(lxrt_prev_task);

#endif /* CONFIG_KBUILD */

---