rtai_sched.c

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

/*
COPYRIGHT (C) 2000  Paolo Mantegazza ([e-mail])

This program is free software; you can redistribute it and/or modify
it under the terms of version 2 of the GNU General Public License as
published by the Free Software Foundation.

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 ([e-mail]) has contributed a very 
  informative proc filesystem procedure.
- Stuart Hughes ([e-mail]) has helped in porting this 
  module to 2.4.xx.
- Stefano Picerno ([e-mail]) for suggesting a simple fix to 
  distinguish a timeout from an abnormal retrun in timed sem waits.
- Geoffrey Martin ([e-mail]) for a fix to functions with timeouts.
 */



#define ALLOW_RR

#define ONE_SHOT 	0
#define PREEMPT_ALWAYS	0
#define LINUX_FPU 	1

#ifndef __MVM__

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/slab.h>
#include <linux/kernel.h>
#include <linux/timex.h>
#include <linux/sched.h>

#include <asm/param.h>
#include <asm/system.h>
#include <asm/io.h>
#include <asm/segment.h>

#ifdef CONFIG_PROC_FS
#include <linux/stat.h>
#include <linux/proc_fs.h>
#include <rtai_proc_fs.h>
#endif

#define INTERFACE_TO_LINUX
#include <rtai.h>
#include <asm/rtai_sched.h>

#include <rtai_sched.h>
#include <rtai_trace.h>

#else /* !__MVM__ */

#define INTERFACE_TO_LINUX
#include "vrtai/rtai_sched.h"
#include "vrtai/rtai_trace.h"

#endif /* !__MVM__ */

MODULE_LICENSE("GPL");

#if defined(CONFIG_RTAI_DYN_MM) || defined(CONFIG_RTAI_DYN_MM_MODULE)
//#include <rt_mem_mgr.h>
#define sched_malloc(size)	rt_malloc((size))
#define sched_free(adr)		rt_free((adr))
#if defined(CONFIG_RTAI_DYN_MM_MODULE)
#define sched_mem_init()
#define sched_mem_end()
#else
#define sched_mem_init() \
	{ if(rt_mem_init() != 0) { \
                printk("Failed to allocate memory for task stack(s)\n"); \
                return(-ENOMEM); \
        } }
#define sched_mem_end()							rt_mem_end()
#endif
#define call_exit_handlers(task)				__call_exit_handlers(task)
#define set_exit_handler(task, fun, arg1, arg2)	__set_exit_handler(task, fun, arg1, arg2)
#else
#define sched_malloc(size)	kmalloc((size), GFP_KERNEL)
#define sched_free(adr)		kfree((adr))
#define sched_mem_init()
#define sched_mem_end()
#define call_exit_handlers(task)
#define set_exit_handler(task, fun, arg1, arg2)
#endif

#define RT_SEM_MAGIC 0xaabcdeff

#define SEM_ERR (0xFfff)

#define MSG_ERR ((RT_TASK *)0xFfff)

#define NOTHING   ((void *)0)

#define SOMETHING ((void *)1)

#define TIMER_FREQ FREQ_8254

#ifdef CONFIG_PROC_FS
static int rtai_proc_sched_register(void);
static void rtai_proc_sched_unregister(void);
#endif

static RT_TASK *rt_current;

static RT_TASK *fpu_task;

static RT_TASK rt_linux_task;

DEFINE_LINUX_CR0

static RTIME rt_time_h;

static int rt_half_tick;

static int oneshot_timer;

static int oneshot_running;

static int shot_fired;

static int preempt_always;

static RT_TASK *wdog_task;

static int rt_next_tid = 1;       /* Next task ID */

#define SEMHLF 0x0000FFFF
#define RPCHLF 0xFFFF0000
#define RPCINC 0x00010000

#define MAX_SRQ  64
static struct { int srq, in, out; void *mp[MAX_SRQ]; } frstk_srq;

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

static inline void enq_ready_edf_task(RT_TASK *ready_task)
{
	RT_TASK *task;
	task = rt_linux_task.rnext;
	while (task->policy < 0 && ready_task->period >= task->period) {
		task = task->rnext;
	}
	task->rprev = (ready_task->rprev = task->rprev)->rnext = ready_task;
	ready_task->rnext = task;
}

static inline void enq_ready_task(RT_TASK *ready_task)
{
	RT_TASK *task;
	task = rt_linux_task.rnext;
	while (ready_task->priority >= task->priority) {
		task = task->rnext;
	}
	task->rprev = (ready_task->rprev = task->rprev)->rnext = ready_task;
	ready_task->rnext = task;
}

static inline int renq_ready_task(RT_TASK *ready_task, int priority)
{
	int retval;
	if ((retval = ready_task->priority != priority)) {
		ready_task->priority = priority;
		(ready_task->rprev)->rnext = ready_task->rnext;
		(ready_task->rnext)->rprev = ready_task->rprev;
		enq_ready_task(ready_task);
	}
	return retval;
}

static inline void rem_ready_task(RT_TASK *task)
{
	if (task->state == READY) {
		(task->rprev)->rnext = task->rnext;
		(task->rnext)->rprev = task->rprev;
	}
}

static inline void rem_ready_current(void)
{
	(rt_current->rprev)->rnext = rt_current->rnext;
	(rt_current->rnext)->rprev = rt_current->rprev;
}

#define TASK_TO_SCHEDULE() \
	do { prio = (new_task = rt_linux_task.rnext)->priority; } while(0);

static inline void enq_timed_task(RT_TASK *timed_task)
{
	RT_TASK *task;
	task = rt_linux_task.tnext;
	while (timed_task->resume_time > task->resume_time) {
		task = task->tnext;
	}
	task->tprev = (timed_task->tprev = task->tprev)->tnext = timed_task;
	timed_task->tnext = task;
}

static inline void wake_up_timed_tasks(void)
{
	RT_TASK *task;
	task = rt_linux_task.tnext;
	while (task->resume_time <= rt_time_h) {
		if ((task->state &= ~(DELAYED | SEMAPHORE | RECEIVE | SEND | RPC | RETURN | MBXSUSP)) == READY) {
			if (task->policy < 0) {
				enq_ready_edf_task(task);
			} else {
				enq_ready_task(task);
			}
		}
		task = task->tnext;
	}
	rt_linux_task.tnext = task;
	task->tprev = &rt_linux_task;
}

static inline void rem_timed_task(RT_TASK *task)
{
	if ((task->state & DELAYED)) {
		(task->tprev)->tnext = task->tnext;
		(task->tnext)->tprev = task->tprev;
	}
}

static void rt_startup(void(*rt_thread)(int), int data)
{
	hard_sti();
	rt_thread(data);
	rt_task_delete(rt_current);
}

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

	if (task->magic == RT_TASK_MAGIC || priority < 0) {
		return -EINVAL;
	}
// If the task struct is unaligned, we'll get problems later
	if ((unsigned long)task & 0xf){
		return -EFAULT;
	}
#ifndef CONFIG_RTAI_FPU_SUPPORT
	if (uses_fpu) {
		return -EINVAL;
	}
#endif
	if (!(st = (int *)sched_malloc(stack_size))) {
		return -ENOMEM;
	}
	if (wdog_task && wdog_task != task && priority == RT_HIGHEST_PRIORITY) {
	    	rt_printk("Highest priority reserved for RTAI watchdog\n");
	    	return -EBUSY;
	}

	memset(task, 0, sizeof(*task));

	task->bstack = task->stack = (int *)(((unsigned long)st + stack_size - 0x10) & ~0xF);
        task->stack[0] = 0;
	task->uses_fpu = uses_fpu ? 1 : 0;
	*(task->stack_bottom = st) = 0;
	task->runnable_on_cpus = 1;
        task->lnxtsk = 0;
	task->magic = RT_TASK_MAGIC; 
	task->policy = 0;
	task->suspdepth = 1;
	task->state = (SUSPENDED | READY);
	task->owndres = 0;
	task->priority = task->base_priority = priority;
	task->prio_passed_to = 0;
	task->period = 0;
	task->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 = NOTHING;        
	task->tprev = task->tnext = 
	task->rprev = task->rnext = task;      
	task->blocked_on = NOTHING;        
	task->signal = signal;
        for (i = 0; i < RTAI_NR_TRAPS; i++) {
                task->task_trap_handler[i] = NULL;
        }
        task->tick_queue        = NOTHING;
        task->trap_handler_data = NOTHING;
	task->resync_frame = 0;
	task->ExitHook = 0;
	task->tid = rt_next_tid++;
	TRACE_RTAI_TASK(TRACE_RTAI_EV_TASK_INIT, task->tid,
			(uint32_t)rt_thread, priority);
	init_arch_stack();

	hard_save_flags_and_cli(flags);
	init_fp_env();
	rt_linux_task.prev->next = task;
	task->prev = rt_linux_task.prev;
	task->next = 0;
	rt_linux_task.prev = task;
	hard_restore_flags(flags);

	return 0;
}


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


void rt_set_runnable_on_cpus(RT_TASK *task, unsigned int runnable_on_cpus) { }


void rt_set_runnable_on_cpuid(RT_TASK *task, unsigned int cpuid) { }


int rt_check_current_stack(void)
{
	char *sp;

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


void rt_set_sched_policy(RT_TASK *task, int policy, int rr_quantum_ns)
{
	if ((task->policy = policy ? 1 : 0)) {
		task->rr_quantum = nano2count(rr_quantum_ns); 
		if ((task->rr_quantum & 0xF0000000) || !task->rr_quantum) {
			task->rr_quantum = rt_times.linux_tick; 
		}
		task->rr_remaining = task->rr_quantum;
		task->yield_time = 0; 
	}
}


#ifdef ALLOW_RR
#define RR_YIELD() \
if (rt_current->policy > 0) { \
	rt_current->rr_remaining = rt_current->yield_time - rt_times.tick_time; \
	if (rt_current->rr_remaining <= 0) { \
		rt_current->rr_remaining = rt_current->rr_quantum; \
		if (rt_current->state == 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; \
			} \
		} \
	} \
}

#define RR_SETYT() \
	if (new_task->policy > 0) { \
		new_task->yield_time = rt_time_h + new_task->rr_remaining; \
	}

#define RR_SPREMP() \
	if (new_task->policy > 0) { \
		preempt = 1; \
		if (new_task->yield_time < intr_time) { \
			intr_time = new_task->yield_time; \
		} \
	} else { \
		preempt = 0; \
	}

#define RR_TPREMP() \
	if (new_task->policy > 0) { \
		preempt = 1; \
		if (new_task->yield_time < rt_times.intr_time) { \
			rt_times.intr_time = new_task->yield_time; \
		} \
	} else { \
		preempt = preempt_always || prio == RT_LINUX_PRIORITY; \
	}

#else
#define RR_YIELD()

#define RR_SETYT()

#define RR_SPREMP() \
do { preempt = 0; } while (0);

#define RR_TPREMP() \
do { preempt = preempt_always || prio == RT_LINUX_PRIORITY; } while (0);
#endif

#define ANTICIPATE

static void rt_schedule(void)
{
	RT_TASK *task, *new_task;
	RTIME intr_time, now;
	int prio, delay, preempt;

	prio = RT_LINUX_PRIORITY;
	task = new_task = &rt_linux_task;
	RR_YIELD();
	if (oneshot_running) {
#ifdef ANTICIPATE
		rt_time_h = rdtsc() + rt_half_tick;
		wake_up_timed_tasks();
#endif
		TASK_TO_SCHEDULE();
		RR_SETYT();

		intr_time = shot_fired ? rt_times.intr_time :
			    rt_times.intr_time + rt_times.linux_tick;
		RR_SPREMP();
		task = &rt_linux_task;
		while ((task = task->tnext) != &rt_linux_task) {
			if (task->priority <= prio && task->resume_time < intr_time) {
				intr_time = task->resume_time;
				preempt = 1;
				break;
			}
		}
		if (preempt || (!shot_fired && prio == RT_LINUX_PRIORITY)) {
			shot_fired = 1;
			if (preempt) {
				rt_times.intr_time = intr_time;
			}
			delay = (int)(rt_times.intr_time - (now = rdtsc())) - tuned.latency;
			if (delay >= tuned.setup_time_TIMER_CPUNIT) {
				delay = imuldiv(delay, TIMER_FREQ, tuned.cpu_freq);
			} else {
				delay = tuned.setup_time_TIMER_UNIT;
				rt_times.intr_time = now + (tuned.setup_time_TIMER_CPUNIT);
			}
			rt_set_timer_delay(delay);
		}
	} else {
		TASK_TO_SCHEDULE();
		RR_SETYT();
	}

	if (new_task != rt_current) {
		if (rt_current == &rt_linux_task) {
			rt_switch_to_real_time(0);
			save_cr0_and_clts(linux_cr0);
		}
		if (new_task->uses_fpu) {
			enable_fpu();
			if (new_task != fpu_task) {
				save_fpenv(fpu_task->fpu_reg);
				fpu_task = new_task;
				restore_fpenv(fpu_task->fpu_reg);
			}
		}
		if (new_task == &rt_linux_task) {
			rt_switch_to_linux(0);
			restore_cr0(linux_cr0);
		}

		TRACE_RTAI_SCHED_CHANGE(rt_current->tid, new_task->tid, rt_current->state);

		rt_switch_to(new_task);
		if (rt_current->signal) {
			(*rt_current->signal)();
		}
	}
	return;
}


int rt_get_prio(RT_TASK *task)
{
	if (task->magic != RT_TASK_MAGIC) {
		return -EINVAL;
	}
	return task->base_priority;
}


int rt_get_inher_prio(RT_TASK *task)
{
	if (task->magic != RT_TASK_MAGIC) {
		return -EINVAL;
	}
	return task->base_priority;
}


void rt_spv_RMS(int cpuid)
{
	RT_TASK *task;
	int prio;
	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;
			}
		}