rtai_sched.c

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

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

int rt_change_prio(RT_TASK *task, int priority)
{
	unsigned long flags;
	int prio, sched;

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

	sched = 0;
	prio = task->base_priority;
	hard_save_flags_and_cli(flags);
	if ((task->base_priority = priority) < task->priority) {
		QUEUE *q;
		do {
			task->priority = priority;
			if (task->state == READY) {
				(task->rprev)->rnext = task->rnext;
				(task->rnext)->rprev = task->rprev;
				enq_ready_task(task);
				sched = 1;
			} else if ((q = task->blocked_on) && !((task->state & SEMAPHORE) && ((SEM *)q)->qtype)) {
				(task->queue.prev)->next = task->queue.next;
				(task->queue.next)->prev = task->queue.prev;
				while ((q = q->next) != task->blocked_on && (q->task)->priority <= priority);
				q->prev = (task->queue.prev = q->prev)->next = &(task->queue);
				task->queue.next = q;
				sched = 1;
			}
		} while ((task = task->prio_passed_to) && task->priority > priority);
		if (sched) {
			rt_schedule();
		}
	}
	hard_restore_flags(flags);
	return prio;
}


void rt_sched_lock(void)
{
	unsigned long flags;

	hard_save_flags_and_cli(flags);
	if (rt_current->priority >= 0) {
		rt_current->sched_lock_priority = rt_current->priority;
		rt_current->priority = -1;
	} else {
		rt_current->priority--;
	}
	hard_restore_flags(flags);
}


void rt_sched_unlock(void)
{
	unsigned long flags;

	hard_save_flags_and_cli(flags);
	if (rt_current->priority < 0 && !(++rt_current->priority)) {
		rt_current->priority = rt_current->sched_lock_priority;
		rt_schedule();
	}
	hard_restore_flags(flags);
}


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

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

	TRACE_RTAI_TASK(TRACE_RTAI_EV_TASK_DELETE, task->tid, 0, 0);

	hard_save_flags_and_cli(flags);
	if (!(task->owndres & SEMHLF) || task == rt_current || rt_current->priority == RT_LINUX_PRIORITY) {
		call_exit_handlers(task);
		rem_timed_task(task);
		if (task->blocked_on) {
			(task->queue.prev)->next = task->queue.next;
			(task->queue.next)->prev = task->queue.prev;
			if (task->state & SEMAPHORE) {
				if (!((SEM *)(task->blocked_on))->type) {
					((SEM *)(task->blocked_on))->count++;
				} else {
					((SEM *)(task->blocked_on))->count = 1;
				}
			}
		}
		q = &(task->msg_queue);
		while ((q = q->next) != &(task->msg_queue)) {
			rem_timed_task(q->task);
			if ((q->task)->state != READY && ((q->task)->state &= ~(SEND | RPC | DELAYED)) == READY) {
				enq_ready_task(q->task);
			}
			(q->task)->blocked_on = 0;
		}       
		q = &(task->ret_queue);
		while ((q = q->next) != &(task->ret_queue)) {
			rem_timed_task(q->task);
			if ((q->task)->state != READY && ((q->task)->state &= ~(RETURN | DELAYED)) == READY) {
				enq_ready_task(q->task);
			}
			(q->task)->blocked_on = 0;
		}       
		if (!((task->prev)->next = task->next)) {
			rt_linux_task.prev = task->prev;
		} else {
			(task->next)->prev = task->prev;
		}
		if (fpu_task == task) {
			/* XXX Don't we lose the linux FPU context here? */
			fpu_task = &rt_linux_task;
		}
		frstk_srq.mp[frstk_srq.in] = task->stack_bottom;
		frstk_srq.in = (frstk_srq.in + 1) & (MAX_SRQ - 1);
		task->magic = 0;
		rt_pend_linux_srq(frstk_srq.srq);
		rem_ready_task(task);
		task->state = 0;
		if (task == rt_current) {
			rt_schedule();
		}
	} else {
		task->suspdepth = -0x7FFFFFFF;
	}
	hard_restore_flags(flags);
	return 0;
}


int rt_get_task_state(RT_TASK *task)
{
	return task->state;
}


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

	TRACE_RTAI_TIMER(TRACE_RTAI_EV_TIMER_HANDLE_EXPIRY, 0, 0);

	prio = RT_LINUX_PRIORITY;
	task = new_task = &rt_linux_task;
#ifdef CONFIG_X86_REMOTE_DEBUG
	if (oneshot_timer) {	// Resync after possibly hitting a breakpoint
	    	rt_times.intr_time = rdtsc();
	}
#endif
	rt_times.tick_time = rt_times.intr_time;
	rt_time_h = rt_times.tick_time + rt_half_tick;
	if (rt_times.tick_time >= rt_times.linux_time) {
		rt_times.linux_time += rt_times.linux_tick;
		rt_pend_linux_irq(TIMER_8254_IRQ);
	}
	wake_up_timed_tasks();
	RR_YIELD();
	TASK_TO_SCHEDULE();
	RR_SETYT();

	if (oneshot_timer) {
		rt_times.intr_time = rt_times.linux_time > rt_times.tick_time ?
		rt_times.linux_time : rt_times.tick_time + rt_times.linux_tick;
		RR_TPREMP();

		task = &rt_linux_task;
		while ((task = task->tnext) != &rt_linux_task) {
			if (task->priority <= prio && task->resume_time < rt_times.intr_time) {
				rt_times.intr_time = task->resume_time;
				preempt = 1;
				break;
			}
		}
		if ((shot_fired = preempt)) {
			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 {
		rt_times.intr_time += rt_times.periodic_tick;
		rt_set_timer_delay(0);
	}

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

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


void recover_jiffies(int irq, void *dev_id, struct pt_regs *regs)
{
	hard_cli();
	if (rt_times.tick_time >= rt_times.linux_time) {
		rt_times.linux_time += rt_times.linux_tick;
		rt_pend_linux_irq(TIMER_8254_IRQ);
	}
	hard_sti();
} 


void rt_set_periodic_mode(void)
{
	stop_rt_timer();
	oneshot_timer = oneshot_running = 0;
}


void rt_set_oneshot_mode(void)
{
	stop_rt_timer();
	oneshot_timer = 1;
}


int rt_get_timer_cpu(void)
{
	return -EINVAL;
}


DECLR_8254_TSC_EMULATION;

RTIME start_rt_timer(int period)
{
	unsigned long flags;

	hard_save_flags_and_cli(flags);
	if (oneshot_timer) {
		SETUP_8254_TSC_EMULATION;
		rt_request_timer(rt_timer_handler, 0, 0);
		tuned.timers_tol[0] = rt_half_tick = tuned.latency;
		oneshot_running = shot_fired = 1;
	} else {
		rt_request_timer(rt_timer_handler, period > LATCH ? LATCH : period, 0);
		tuned.timers_tol[0] = rt_half_tick = (rt_times.periodic_tick + 1)>>1;
	}
	rt_time_h = rt_times.tick_time + rt_half_tick;
	hard_restore_flags(flags);
	rt_request_linux_irq(TIMER_8254_IRQ, recover_jiffies, "rtai_jif_chk", recover_jiffies);
	return period;
}


RTIME start_rt_timer_cpuid(int period, int cpuid)
{
	return start_rt_timer(period);
}


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)
{
	unsigned long flags;
	rt_free_linux_irq(TIMER_8254_IRQ, recover_jiffies);
	hard_save_flags_and_cli(flags);
	CLEAR_8254_TSC_EMULATION;
	rt_free_timer();
	oneshot_timer = oneshot_running = 0;
	hard_restore_flags(flags);
}


RT_TASK *rt_whoami(void)
{
	return rt_current;
}


int rt_sched_type(void)
{
	return UP_SCHED;
}


int rt_task_signal_handler(RT_TASK *task, void (*handler)(void))
{
	if (task->magic != RT_TASK_MAGIC) {
		return -EINVAL;
	}

	TRACE_RTAI_TASK(TRACE_RTAI_EV_TASK_SIG_HANDLER, task->tid, (uint32_t) handler, 0);

	task->signal = handler;
	return 0;
}

#ifdef CONFIG_RTAI_FPU_SUPPORT
int rt_task_use_fpu(RT_TASK *task, int use_fpu_flag)
{
	if (task->magic != RT_TASK_MAGIC) {
		return -EINVAL;
	}
	task->uses_fpu = use_fpu_flag ? 1 : 0;
	return 0;
}
#else
int rt_task_use_fpu(RT_TASK *task, int use_fpu_flag)
{ 
	return -EINVAL;
}
#endif

void rt_linux_use_fpu(int use_fpu_flag)
{
	rt_linux_task.uses_fpu = use_fpu_flag ? 1 : 0;
}


void rt_preempt_always(int yes_no)
{
	preempt_always = yes_no ? 1 : 0;
}


void rt_preempt_always_cpuid(int yes_no, unsigned int cpuid)
{
	rt_preempt_always(yes_no);
}


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


int rt_trap_handler(int vec, int signo, struct pt_regs *regs, void *dummy_data)
{
        if (!rt_current) {
		return 0;
	}

	if (rt_current->task_trap_handler[vec]) {
                return rt_current->task_trap_handler[vec]( vec,
                                                           signo,
                                                           regs,
                                                           rt_current);
	}

	rt_printk("Default Trap Handler: vector %d: Suspend RT task %p\n", vec,
rt_current);
	rt_task_suspend(rt_current);
	rt_task_delete(rt_current); // In case the suspend does not work ?

        return 1;
}

#ifndef CONFIG_RTAI_DYN_MM_MODULE
extern unsigned int granularity;
MODULE_PARM(granularity, "i");

extern int low_chk_ref;
MODULE_PARM(low_chk_ref, "i");

extern int low_data_mark;
MODULE_PARM(low_data_mark, "i");
#endif

static int OneShot = ONE_SHOT;
MODULE_PARM(OneShot, "i");

static int Preempt_Always = PREEMPT_ALWAYS;
MODULE_PARM(Preempt_Always, "i");

static int LinuxFpu = LINUX_FPU;
MODULE_PARM(LinuxFpu, "i");

static int Latency = LATENCY_8254;
MODULE_PARM(Latency, "i");

static int SetupTimeTIMER = SETUP_TIME_8254;
MODULE_PARM(SetupTimeTIMER, "i");

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

int init_module(void)
{
	sched_mem_init();
	rt_linux_task.uses_fpu = LinuxFpu ? 1 : 0;
	rt_linux_task.magic = 0;
	rt_linux_task.policy = 0;
	rt_linux_task.state = READY;
	rt_linux_task.priority = rt_linux_task.base_priority = RT_LINUX_PRIORITY;
	rt_linux_task.signal = 0;
	rt_linux_task.prev = &rt_linux_task;
	rt_linux_task.next = 0;
	rt_linux_task.resume_time = RT_TIME_END;
	rt_linux_task.tprev = rt_linux_task.tnext =
	rt_linux_task.rprev = rt_linux_task.rnext = &rt_linux_task;
	rt_current = &rt_linux_task;
	fpu_task = &rt_linux_task;
	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_timer = OneShot ? 1 : 0;
	oneshot_running = 0;
	preempt_always = Preempt_Always ? 1 : 0;
#ifdef CONFIG_PROC_FS
	rtai_proc_sched_register();
#endif
	printk("\n***** STARTING THE UP REAL TIME SCHEDULER WITH %sLINUX *****", LinuxFpu ? "": "NO ");
	printk("\n***** FP SUPPORT AND READY FOR A %s TIMER *****", oneshot_timer ? "ONESHOT": "PERIODIC");
	printk("\n***<> LINUX TICK AT %d (HZ) <>***", HZ);
	printk("\n***<> CALIBRATED CPU FREQUENCY %d (HZ) <>***", tuned.cpu_freq);
	printk("\n***<> CALIBRATED 8254-TIMER-INTERRUPT-TO-SCHEDULER LATENCY %d (ns) <>***", imuldiv(tuned.latency - tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq));
	printk("\n***<> CALIBRATED ONE SHOT SETUP TIME %d (ns) <>***\n\n", imuldiv(tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq));
	rt_mount_rtai();
// 0x7dd763ad == nam2num("MEMSRQ").
	if ((frstk_srq.srq = rt_request_srq(0x7dd763ad, frstk_srq_handler, 0)) < 0) {
		printk("MEM SRQ: no sysrq available.\n");
		return frstk_srq.srq;
	}
	frstk_srq.in = frstk_srq.out = 0;
	RT_SET_RTAI_TRAP_HANDLER(rt_trap_handler);
	return 0;
}


void cleanup_module(void)
{
	RT_SET_RTAI_TRAP_HANDLER(NULL);
	stop_rt_timer();
	while (rt_linux_task.next) {
		rt_task_delete(rt_linux_task.next);
	}
#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);
	}
	sched_mem_end();
	rt_umount_rtai();
	printk("\n***** THE UP REAL TIME SCHEDULER HAS BEEN REMOVED *****\n\n");
}

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

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

	task = &rt_linux_task;
	PROC_PRINT("\nRTAI Uniprocessor Real Time Task Scheduler.\n\n");
	PROC_PRINT("    Calibrated CPU Frequency: %d Hz\n", tuned.cpu_freq);
	PROC_PRINT("    Calibrated 8254 interrupt to scheduler latency: %d ns\n", imuldiv(tuned.latency - tuned.setup_time_TIMER_CPUNIT, 1000000000, tuned.cpu_freq));
	PROC_PRINT("    Calibrated one shot setup time: %d ns\n\n",