/* TODO
  The problem with task queues is that they optimize for
  large numbers of tasks. I think that large numbers of tasks
  requires a total redesign of scheduler and that the scheduler
  should optimize for < 40 threads and really for <10

  */
/*
 * RTLinux default scheduler
 * RTLinux has a modular scheduler and this may be replaced if desired.
 *
 * Written by Michael Barabanov, Victor Yodaiken
 * Copyright (C) Finite State Machine Labs Inc., 1998,1999
 * Released under the terms of the GNU  General Public License Version 2
 *
 */

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

#include <linux/timex.h>

#include <rtl_conf.h>

#include <rtl_debug.h>

#include <rtl_core.h>
#include <rtl.h>
#include <rtl_sync.h>
#include <rtl_sched.h>
#include <rtl_tqueue.h>
#include <arch/rtl_switch.h>

static spinlock_t rtl_tqueue_lock;


static int rtl_sched_irq;
static pthread_t zombie_threads;

#ifdef CONFIG_SMP
struct rtl_sched_cpu_struct rtl_sched [NR_CPUS];
#else
struct rtl_sched_cpu_struct rtl_sched [1];
#endif

/* # define rtl_printf(fac, args...)	do ; while (0) */

__attribute__ ((regparm(0))) int rtl_startup(void *(*fn)(void *), void *data)
{
	void *retval;

	rtl_allow_interrupts();

#ifdef CONFIG_RTL_FP_SUPPORT
	if (pthread_self()->uses_fp) {
		pthread_self()->uses_fp = 0; /* to force save/restore */
		pthread_setfp_np (pthread_self(), 1);
	}
#endif
	/*
	 * This is needed by the V1 API.   Since we don't do the V1
	 * API on anything but x86 we don't need to do the schedule,
	 * though.
	 *    -- Cort
	 */
	rtl_posix_init(pthread_self());
	rtl_schedule();
	retval = (*fn)(data);
	pthread_exit(retval);

	/* will never reach this line */
	return 0;
}


static inline pthread_t rtl_get_linux_thread(int cpu_id){
	return &(rtl_sched[cpu_id].rtl_linux_task);
}
int rtl_setclockmode (rtl_clockid_t clock, int mode, hrtime_t param)
{
	int ret;
	rtl_irqstate_t flags;
	rtl_no_interrupts (flags);
	ret = clock->settimermode (clock, mode);
	if (ret != 0) {
		rtl_restore_interrupts (flags);
		return ret;
	}
	if (mode == RTL_CLOCK_MODE_ONESHOT) {
		param = HRTIME_INFINITY;
	}
	ret = clock->settimer (clock, param);
	rtl_restore_interrupts (flags);
	return ret;
}


struct task_struct *get_linux_current(void)
{
#if !defined(__i386__)
	return current;
#else
	if ( RTL_CURRENT == rtl_get_linux_thread(rtl_getcpuid()) )
	{
		return current;
	}
	else
	{
		int *sp = rtl_get_linux_thread(rtl_getcpuid())->stack;
		return (struct task_struct *)((ulong)sp & (ulong)~8191UL);
	}
#endif	
}

static void rtl_reschedule_thread(pthread_t t){
#ifdef CONFIG_SMP                                                                         
		if (t->cpu != rtl_getcpuid())
			rtl_reschedule (t->cpu);
		else
#endif
			rtl_schedule();
}

/* Note: as per POSIX standard, delivery of the signal is not
   necessarily soon. In RTLinux it waits until the next scheduling
   you must do sigqueue(cpu_id,RTL_RESCHED_SIGNAL) to get it to work.
   but TODO sigqueue
 
   Note that currently no soft signals require any real processing so we 
   don't need to be "in the context" of the receiving thread.
   Signals are hard: meaning we get them in the process context
   anyways (on top of whatever thread is running) or schedule control
   which means that we just manipulate the scheduler status and be done.


 */

#define CHECK_VALID(thread) do { if ((thread)->magic != RTL_THREAD_MAGIC) return ESRCH; } while (0)

int pthread_kill(pthread_t thread, int signal)
{
	if ((unsigned) signal <= RTL_MAX_SIGNAL) {
			CHECK_VALID(thread);
			if (signal == 0) {
				return 0;
			}
			set_bit(signal, (volatile unsigned long *)&thread->pending);
			return 0;
	} else if(thread != rtl_get_linux_thread(rtl_getcpuid())) {
		return EINVAL;
	} else { /* the signal number means something else */
		/* TODO one range for local one for global */
		if((signal < RTL_LINUX_MIN_SIGNAL)
			       	|| (signal > RTL_LINUX_MAX_SIGNAL))
			return EINVAL;
		else if(signal < RTL_LINUX_MIN_LOCAL_SIGNAL){ /* it's global */
			rtl_global_pend_irq (signal - RTL_LINUX_MIN_SIGNAL );
		}
#ifdef CONFIG_SMP
		else{
		       	rtl_local_pend_vec(signal - RTL_LINUX_MIN_LOCAL_SIGNAL,rtl_getcpuid());
		}
#endif
		return 0;
	}
}



static void do_cleanups(pthread_t t)
{
	struct rtl_cleanup_struct *ts = t->cleanup;
	while ((ts = t->cleanup)) {
		t->cleanup = ts->next;
		ts->routine(ts->arg);
	}
}

/* call with interrupts disabled */
static int remove_from_this_cpu(pthread_t thread)
{
	int found = 0;
	struct rtl_thread_struct *t;
	schedule_t *s;
	s = LOCAL_SCHED;
#ifdef CONFIG_SMP
	{
		unsigned int cpu_id = rtl_getcpuid();
		if (thread->cpu != cpu_id){
		        rtl_printf("RTLinux ERROR: remove_from this cpu crosses CPUs\n");       
			return ESRCH;
		}
	}
#endif
	rtl_spin_lock (&s->rtl_tasks_lock);
	if (thread != s->rtl_tasks) {
		for (t = s->rtl_tasks; t; t = t->next) {
			if (t->next == thread) {
				t->next = thread->next;
				found = 1;
				break;
			}
		}
		if (!found) {
			rtl_spin_unlock (&s->rtl_tasks_lock);
			return ESRCH;
		}
	} else {
		s->rtl_tasks = thread->next;
	}

	if (s->rtl_task_fpu_owner == thread) {
		s->rtl_task_fpu_owner = 0;
	}

	rtl_spin_unlock (&s->rtl_tasks_lock);
	return 0;
}

void rtl_task_free_memory(struct work_struct *p)
{
	struct rtl_thread_struct *task;

	task = container_of(p, struct rtl_thread_struct, free_task);
	
	if (task->kmalloc_stack_bottom) {
/* 		rtl_printf("freeing %#x %#x\n", task->kmalloc_stack_bottom); */
		kfree (task->kmalloc_stack_bottom);
	}
}


static int delete_thread(pthread_t thread)
{
	int ret;
	rtl_irqstate_t interrupt_state;
	rtl_no_interrupts (interrupt_state);

	memset(&thread->free_task, 0, sizeof(free_task));

	INIT_WORK(&thread->free_task, rtl_task_free_memory); 

	thread->cleanup = (struct rtl_cleanup_struct *) &thread->free_task;

	thread->magic = 0;
	ret = remove_from_this_cpu (thread);
	if (ret != 0) {
		rtl_restore_interrupts (interrupt_state);
		return ret;
	}

	spin_lock(&rtl_tqueue_lock);
	thread->next = zombie_threads;
	zombie_threads = thread;
	spin_unlock(&rtl_tqueue_lock);

	rtl_global_pend_irq (rtl_sched_irq);
	
	pthread_suspend_np(pthread_self());

	rtl_restore_interrupts (interrupt_state);

	return 0;
}

void pthread_exit(void *retval)
{
	rtl_irqstate_t flags;
	pthread_t self = pthread_self();

	rtl_no_interrupts(flags);
	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);

	do_cleanups(self);
	rtl_posix_cleanup(retval);

	set_bit (RTL_THREAD_FINISHED, (volatile unsigned long *)&self->threadflags);
	delete_thread(pthread_self());

	rtl_schedule();
	/* will never reach this line */
	rtl_restore_interrupts(flags);
	rtl_printf("pthread_exit() returned!\n");
}


int sched_yield(void)
{
	rtl_schedule();
	return 0;
}

int pthread_make_periodic_np (pthread_t p, hrtime_t start_time, hrtime_t period)
{
	rtl_irqstate_t interrupt_state;
	if (period < 0) {
		return EINVAL;
	}

	rtl_no_interrupts(interrupt_state);

	p->period = period;
	__rtl_setup_timeout(p, start_time);

	rtl_reschedule_thread(p);
	rtl_restore_interrupts(interrupt_state);
	return 0;
}


int pthread_setperiod_np(pthread_t p, const struct itimerspec *value )
{
#ifdef CONFIG_SMP
	{
		unsigned int cpu_id = rtl_getcpuid();
		if (p->cpu != cpu_id){
		        rtl_printf("RTLinux ERROR: pthread_setperiod crosses cpus\n");       
			return ESRCH;
		}
	}
#endif
	if (!timespec_nz(&value->it_value)) {
		pthread_make_periodic_np (p, HRTIME_INFINITY, 0);
	} else {
		pthread_make_periodic_np (p, rtl_timespec_to_ns (&value->it_value),
				rtl_timespec_to_ns (&value->it_interval));
	}
	return 0;
}


/* TODO */
int timer_gettime(timer_t timerid, struct itimerspec *value)
{
	return EINVAL;
}

/* TODO */
int timer_getoverrun(timer_t timerid)
{
	return EINVAL;
}



inline static struct rtl_thread_struct * find_preemptor(schedule_t *s, struct rtl_thread_struct *chosen){
	struct rtl_thread_struct *t;
	struct rtl_thread_struct *preemptor=0;
	for (t = s->rtl_tasks; t; t = t->next) {
		if (test_bit(RTL_THREAD_TIMERARMED, (volatile unsigned long *)&t->threadflags)) {
			if (t->sched_param.sched_priority > chosen->sched_param.sched_priority)
			{
				if(!preemptor ||(t->resume_time < preemptor->resume_time))
				{	
					preemptor = t;
				}
			}
		}
	}
	return preemptor;
}

#define do_abort(t) do { clear_bit(RTL_THREAD_TIMERARMED, (volatile unsigned long *)&t->threadflags); if (t->abort) { t->abort(t->abortdata); }} while (0)


static void rtl_posix_cancel(int signal)
{
	set_bit (RTL_CANCELPENDING, (volatile unsigned long *)&pthread_self()->threadflags);
	if (test_bit (RTL_CANCELTYPE, (volatile unsigned long *)&pthread_self()->threadflags)) {
		pthread_testcancel();
	}
}


static void (*rtl_sigcancel_handler)(int signal) = rtl_posix_cancel;

static inline void do_signal(pthread_t t)
{
	if (test_and_clear_bit(RTL_SIGNAL_SUSPEND, (volatile unsigned long *)&t->pending)) {
		t->abort = 0;
		t->abortdata = 0;
		do_abort(t);
		RTL_MARK_SUSPENDED(t);
		rtl_schedule();
	}

	if (test_and_clear_bit(RTL_SIGNAL_WAKEUP, (volatile unsigned long *)&t->pending)) {
		RTL_MARK_READY(t);
		do_abort(t);
	}

	if (test_and_clear_bit(RTL_SIGNAL_TIMER, (volatile unsigned long *)&t->pending)) {
		RTL_MARK_READY(t);
		do_abort(t);
	}

	if (!test_bit(RTL_SIGNAL_CANCEL, (volatile unsigned long *)&t->blocked) && test_and_clear_bit(RTL_SIGNAL_CANCEL, (volatile unsigned long *)&t->pending)) {
		RTL_MARK_READY(t);
		do_abort(t);
		(*rtl_sigcancel_handler)(RTL_SIGNAL_CANCEL);
	}
}


int rtl_schedule (void)
{
	schedule_t *sched;
	struct rtl_thread_struct *t;
	struct rtl_thread_struct *new_task;
	struct rtl_thread_struct *preemptor = 0; 
	unsigned long interrupt_state;
	int cpu_id = rtl_getcpuid();
	rtl_sigset_t mask;	
	hrtime_t now;

	rtl_no_interrupts(interrupt_state);
	rtl_trace2 (RTL_TRACE_SCHED_IN, (long) pthread_self());
	/* new_task = &sched->rtl_linux_task;*/

	new_task = 0;
	sched = &rtl_sched[cpu_id];

	now = sched->clock->gethrtime(sched->clock);

	if (sched->clock->mode == RTL_CLOCK_MODE_ONESHOT) {
		sched->clock->value = now;
	}

	for (t = sched->rtl_tasks; t; t = t->next) {
		/* expire timers */

		if (test_bit(RTL_THREAD_TIMERARMED, (volatile unsigned long *)&t->threadflags)) {
			if (now >= t->resume_time) {
				clear_bit(RTL_THREAD_TIMERARMED, (volatile unsigned long *)&t->threadflags);
				rtl_sigaddset (&t->pending, RTL_SIGNAL_TIMER);
				if (t->period != 0) { /* periodic */
					t->resume_time += t->period;
					/* timer overrun */
#ifndef CONFIG_RTL_OLD_TIMER_BEHAVIOUR
					while (now >= t->resume_time) {
						t->resume_time += t->period;
/* 						rtl_printf("overrun"); */
					}
#endif
				} else {
					t->resume_time = HRTIME_INFINITY;
				}
			}
		}
		
		/* and find highest priority runnable task */
		if ((t->pending & ~t->blocked) && (!new_task ||
			(t->sched_param.sched_priority > new_task->sched_param.sched_priority))) {
				new_task = t;
		}
	}

	if (sched->clock->mode == RTL_CLOCK_MODE_ONESHOT && !test_bit (RTL_SCHED_TIMER_OK, (volatile unsigned long *)&sched->sched_flags)) {
		if ( (preemptor = find_preemptor(sched,new_task))) {
			(sched->clock)->settimer(sched->clock, preemptor->resume_time - now);
		} else {
			(sched->clock)->settimer(sched->clock, (HRTICKS_PER_SEC / HZ) / 2);
		}
		set_bit (RTL_SCHED_TIMER_OK, (volatile unsigned long *)&sched->sched_flags);
	}

	if (new_task != sched->rtl_current) { /* switch out old, switch in new */
		if (new_task ==  &sched->rtl_linux_task) {
			rtl_make_rt_system_idle();

		} else {
			rtl_make_rt_system_active(); 
		}

		rtl_trace2 (RTL_TRACE_SCHED_CTX_SWITCH, (long) new_task);
		rtl_switch_to(&sched->rtl_current, new_task);

		/* delay switching the FPU context until it is really needed */
#ifdef CONFIG_RTL_FP_SUPPORT
		if (sched->rtl_current-> uses_fp &&\
			       	sched->rtl_task_fpu_owner != sched->rtl_current)
		{
			if (sched->rtl_task_fpu_owner)
			{
				rtl_fpu_save (sched,sched->rtl_task_fpu_owner);
			}
			rtl_fpu_restore (sched,sched->rtl_current);
			sched->rtl_task_fpu_owner = sched->rtl_current;
		}
#endif /* CONFIG_RTL_FP_SUPPORT */
	}
/*	if (RTL_CURRENT==rtl_get_linux_thread(rtl_getcpuid())) {
		unsigned long flags;
		rtl_allow_interrupts();
		__save_flags(flags);
		__restore_flags(flags);
	}*/
	mask = pthread_self()->pending;

	if (pthread_self()->pending & ~(1 << RTL_SIGNAL_READY)) do_signal(pthread_self());

	rtl_trace2 (RTL_TRACE_SCHED_OUT, (long) pthread_self());
	rtl_restore_interrupts(interrupt_state);
	return mask;
}


int pthread_suspend_np (pthread_t thread)
{
	if (thread == pthread_self()) {
		RTL_MARK_SUSPENDED (pthread_self());
		rtl_schedule();
		pthread_testcancel();
	} else {
		pthread_kill(thread,RTL_SIGNAL_SUSPEND);
	}
	return 0;
}

int pthread_wakeup_np (pthread_t thread)
{
#ifdef CONFIG_SMP
	if (thread->cpu != rtl_getcpuid()) {
		pthread_kill(thread,RTL_SIGNAL_WAKEUP);
		rtl_reschedule_thread(thread);
	} else
#endif
	{
		pthread_kill(thread,RTL_SIGNAL_WAKEUP);
		rtl_reschedule_thread(thread);
	}
	return 0;
}


int pthread_attr_setcpu_np(pthread_attr_t *attr, int cpu)
{
#if 0
	if (!rtl_cpu_exists(cpu)) {
		return EINVAL;
	}
#endif
	attr->cpu = cpu;
	return 0;
}

int pthread_attr_setstackaddr(pthread_attr_t *attr, void *stackaddr)
{
	attr->stack_addr = stackaddr;
	return 0;
}

int pthread_attr_getstackaddr(const pthread_attr_t *attr, void **stackaddr)
{
	*stackaddr = attr->stack_addr;
	return 0;
}

static void add_to_task_list(pthread_t thread)
{
	schedule_t *s = LOCAL_SCHED;
	thread->next = s -> rtl_tasks;
	s->rtl_tasks = thread;
}
 

int __pthread_create (pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg, struct module *creator)
{
	int *stack_addr;
	long interrupt_state;
	struct rtl_thread_struct *task;
	pthread_attr_t default_attr;
	int stack_size;
	if (!attr) {
		pthread_attr_init(&default_attr);
		attr = &default_attr;
	}

	stack_size = attr->stack_size;
	stack_addr = (int *) attr->stack_addr;

	if (!stack_addr) {
		if (pthread_self() != pthread_linux()) {
			return EAGAIN;
		}
		stack_addr = (int *) kmalloc(stack_size, GFP_KERNEL);
 		task = (struct rtl_thread_struct *) stack_addr;
 		if (!task) {
  			return EAGAIN;
  		}
		task->kmalloc_stack_bottom = stack_addr;
	} else {
		task = (struct rtl_thread_struct *) stack_addr;
		task->kmalloc_stack_bottom = 0;
	}

	*thread = task;
	printk("thread at %x stack at %x\n", (unsigned int)thread, (unsigned int)*thread);

	task -> threadflags = 0;
	if (attr->detachstate == PTHREAD_CREATE_JOINABLE) {
		set_bit(RTL_THREAD_JOINABLE, (volatile unsigned long *)&task->threadflags);
	}

	task->magic = RTL_THREAD_MAGIC;
	task->creator = creator;
	task->pending = attr->initial_state;
	task->blocked = 0;

	task->abort = 0;

	task->cpu = attr->cpu;

	task->cleanup = 0;
	task->resume_time = HRTIME_INFINITY;
	task->period = 0;
	task->sched_param = attr->sched_param;
	task->stack = stack_addr + stack_size / sizeof(int);
	rtl_init_stack(task,start_routine, arg,rtl_startup);
	rtl_no_interrupts(interrupt_state);

	task->fpu_initialized = 0;
	task->uses_fp = attr->use_fp;

	{
	schedule_t *s = sched_data(task->cpu);
#ifdef CONFIG_SMP
	if (task->cpu != rtl_getcpuid()) {
		rtl_spin_lock (&s->rtl_tasks_lock);
		task->next = s -> rtl_new_tasks;
		s->rtl_new_tasks = task;
		rtl_spin_unlock (&s->rtl_tasks_lock);
		rtl_reschedule (task->cpu);
	} else
#endif
	{
		rtl_spin_lock (&s->rtl_tasks_lock);
		add_to_task_list(task);
		rtl_spin_unlock (&s->rtl_tasks_lock);
		rtl_schedule();
	}
	}

	rtl_restore_interrupts(interrupt_state);
	return 0;
}


#ifdef CONFIG_RTL_FP_SUPPORT
int pthread_attr_setfp_np (pthread_attr_t *attr, int flag)
{
	attr->use_fp = flag;
	return 0;
}

int pthread_setfp_np (pthread_t thread, int flag)
{
	schedule_t *sched = LOCAL_SCHED;
	rtl_irqstate_t flags;
#ifdef CONFIG_SMP
	if (rtl_getcpuid() != thread->cpu) {
		rtl_printf("pthread_setfp_np() called on a wrong CPU!\n");
		return EINVAL;
	}
#endif
	rtl_no_interrupts(flags);

#ifdef CONFIG_RTL_FP_SUPPORT
	if (thread -> uses_fp != flag) {
		thread -> uses_fp = flag;
		if (flag) {
			if (thread == pthread_self()) {
				if (sched->rtl_task_fpu_owner) {
					rtl_fpu_save (sched, sched->rtl_task_fpu_owner);
				}
				rtl_fpu_restore (sched, thread);
				sched->rtl_task_fpu_owner = thread;
			}
		}/*  else {
			if (sched->rtl_task_fpu_owner == thread) {
				sched->rtl_task_fpu_owner = 0;
			}
		} */
	}
#endif /* CONFIG_RTL_FP_SUPPORT */

	rtl_restore_interrupts(flags);
	return 0;
}
#endif

static void rtl_sched_timer_interrupt( struct pt_regs *regs)
{
	clear_bit (RTL_SCHED_TIMER_OK, (volatile unsigned long *)&LOCAL_SCHED->sched_flags); \
	rtl_schedule();
}

int pthread_setcancelstate(int state, int *oldstate)
{
	if (oldstate) {
		*oldstate = rtl_sigismember (&pthread_self()->blocked, RTL_SIGNAL_CANCEL);
	}
	if (state) {
		set_bit (RTL_SIGNAL_CANCEL, (volatile unsigned long *)&pthread_self()->blocked);
	} else {
		clear_bit (RTL_SIGNAL_CANCEL, (volatile unsigned long *)&pthread_self()->blocked);
	}
	return 0;
}


int pthread_setcanceltype(int type, int *oldtype)
{
	rtl_irqstate_t flags;
	rtl_no_interrupts (flags);
	if (oldtype) {
		*oldtype = test_bit(RTL_CANCELTYPE, (volatile unsigned long *)&pthread_self()->threadflags);
	}

	if (type) {
		set_bit (RTL_CANCELTYPE, (volatile unsigned long *)&pthread_self()->threadflags);
	} else {
		clear_bit (RTL_CANCELTYPE, (volatile unsigned long *)&pthread_self()->threadflags);
	}
	rtl_restore_interrupts(flags);
	return 0;
}

int pthread_cancel (pthread_t thread)
{
	if (RTL_PRIO(thread) < RTL_PRIO(pthread_linux())) {
		RTL_PRIO(thread) = RTL_PRIO(pthread_linux()) + 3;
	}
	return pthread_kill(thread, RTL_SIGNAL_CANCEL);
}

void pthread_testcancel(void)
{
	if (test_and_clear_bit(RTL_CANCELPENDING, (volatile unsigned long *)&pthread_self()->threadflags)) {
		pthread_exit(PTHREAD_CANCELED);
	}
}


int pthread_delete_np (pthread_t thread)
{
	hrtime_t timeout = gethrtime() + 1000000000;
	clear_bit (RTL_SIGNAL_CANCEL, (volatile unsigned long *)&thread->blocked);
	set_bit (RTL_CANCELTYPE, (volatile unsigned long *)&thread->threadflags);
	set_bit(RTL_CANCELPENDING, (volatile unsigned long *)&thread->threadflags);
	clear_bit(RTL_THREAD_JOINABLE, (volatile unsigned long *)&thread->threadflags);
	/* now the cancel is enabled+asynchronous */
	pthread_cancel (thread);
	while (!test_bit (RTL_THREAD_FINISHED, (volatile unsigned long *)&thread->threadflags)) {
		barrier();
		if (gethrtime() >= timeout) {
			rtl_printf("timed out in pthread_delete_np!\n");
			break;
		}
	}
	return 0;
}


#ifdef RTL_DEBUG
void rtl_dump_tasks(void)
{
	schedule_t *sched = LOCAL_SCHED;
	struct rtl_thread_struct *t;
	hrtime_t ts = sched->clock->gethrtime(sched->clock);
	rtl_printf("Tasks on CPU %d time = (%9d)\n", rtl_getcpuid(), ts);
	for (t = sched->rtl_tasks; t; t = t->next) {
		if (t == &sched->rtl_linux_task) {
			rtl_printf("linux task ");
		}
		rtl_printf("addr=%08x state=%04x i=(%9d) p=(%9d)\n", t, t->state, t->resume_time, t->period);
	}
}
#endif

int pthread_wait_np(void)
{
	long interrupt_state;
	pthread_t self = pthread_self();
	rtl_no_interrupts(interrupt_state);
	RTL_MARK_SUSPENDED (self);
	__rtl_setup_timeout (self, self->resume_time);
	rtl_schedule();
	pthread_testcancel();
	rtl_restore_interrupts(interrupt_state);
	return 0;
}


#ifdef CONFIG_SMP
static unsigned int resched_irq(struct pt_regs *r)
{
	pthread_t t;
	pthread_t nexttask;
/* 	rtl_printf(" r on cpu %d; rtl_new_tasks = %x\n", rtl_getcpuid(), LOCAL_SCHED->rtl_new_tasks); */
	rtl_spin_lock (&LOCAL_SCHED->rtl_tasks_lock);
	for (t = LOCAL_SCHED->rtl_new_tasks; t; t = nexttask) {
		nexttask = t->next;
		add_to_task_list(t); /* modifies t->next */
	}
	LOCAL_SCHED->rtl_new_tasks = 0;
	rtl_spin_unlock (&LOCAL_SCHED->rtl_tasks_lock);
	rtl_schedule();
	
	return 0;
}
#endif /* CONFIG_SMP */

irqreturn_t sched_irq_handler (int irq, void *dev_id, struct pt_regs *p)
{
	rtl_irqstate_t flags;
	rtl_spin_lock_irqsave(&rtl_tqueue_lock, flags);
	while (zombie_threads) {
		pthread_t th = zombie_threads;
		zombie_threads = zombie_threads->next;
		rtl_spin_unlock_irqrestore(&rtl_tqueue_lock, flags);

		rtl_posix_on_delete(th);
		if (th->kmalloc_stack_bottom) {
			rtl_schedule_task ((struct work_struct *) th->cleanup);
		}
		rtl_spin_lock_irqsave(&rtl_tqueue_lock, flags);
	}
	rtl_spin_unlock_irqrestore(&rtl_tqueue_lock, flags);
	
	return 0;
}


static int *thread_errno_location(void)
{
	return &(RTL_CURRENT->errno_val);
}
static int *(*save_errno_location)(void);
		 
int init_module(void)
{
	rtl_irqstate_t interrupt_state;
	int i = 0;
	int ret;
	int my_cpu_id;
	schedule_t *s;
	unsigned int cpu_id = rtl_getcpuid();
	
	printk("rtl_startup: %x\n", (unsigned int)&rtl_startup);
	rtl_spin_lock_init (&rtl_tqueue_lock);
	zombie_threads = 0;
	ret  = rtl_get_soft_irq (sched_irq_handler, "RTLinux Scheduler");
	if (ret > 0) {
		rtl_sched_irq = ret ;
	} else {
		rtl_printf ("Can't get an irq for RTLinux scheduler");
		return -EINVAL;
	}

	rtl_no_interrupts(interrupt_state);

	my_cpu_id = cpu_id;

	/* This next code is a loop if more than one cpu */

		cpu_id = cpu_logical_map (i);
		s = &rtl_sched [cpu_id];
		s -> rtl_current = &s->rtl_linux_task;
		s -> rtl_tasks = &s->rtl_linux_task;
		s -> rtl_new_tasks = 0;

		rtl_spin_lock_init (&s->rtl_tasks_lock);

		s -> rtl_linux_task . magic = RTL_THREAD_MAGIC;
		rtl_sigemptyset(&s -> rtl_linux_task . pending);
		rtl_sigaddset(&s -> rtl_linux_task . pending, RTL_SIGNAL_READY);
		s -> rtl_linux_task . blocked = 0;
		s -> rtl_linux_task . threadflags = 0;
		s -> rtl_linux_task . sched_param . sched_priority = -1;
		s -> rtl_linux_task . next = 0;
		s -> rtl_linux_task . uses_fp = 1;
		s -> rtl_linux_task . fpu_initialized = 1;
		s -> rtl_linux_task . creator = 0;
		s -> rtl_linux_task . abort = 0;

		s -> rtl_task_fpu_owner = &s->rtl_linux_task;
		s -> sched_flags = 0;
		rtl_posix_init (&s->rtl_linux_task);

	        s-> clock =  rtl_getbestclock (cpu_id);
		if (s->clock && rtl_setclockhandler (s->clock, rtl_sched_timer_interrupt) == 0) {
			s->clock->init(s->clock);
		} else {
			rtl_printf("Can't get a clock for processor %d\n",cpu_id);
			rtl_restore_interrupts (interrupt_state);
			return  -EINVAL;
		}

	cpu_id = my_cpu_id;

	rtl_restore_interrupts (interrupt_state);
	save_errno_location = __errno_location;
	__errno_location = thread_errno_location;

/* 	rtl_setdebug (RTLDBG_ALL); */
	return 0;
}


void cleanup_module(void)
{
	int i = 0;
	int cpu;
	schedule_t *s;

	__errno_location = save_errno_location;
	rtl_free_soft_irq(rtl_sched_irq);
	//for (i = 0; i < rtl_num_cpus(); i++) {
		cpu = cpu_logical_map (i);
		s = &rtl_sched [cpu];
		s->clock->uninit(s->clock);
#if 0
		rtl_free_ipi(cpu);
#endif
	//}
}

int usleep (useconds_t interval)
{
	rtl_irqstate_t flags;
	pthread_t th = pthread_self();
	hrtime_t save_resume_time;

	rtl_no_interrupts (flags);
	save_resume_time = th->resume_time;

	RTL_MARK_SUSPENDED(th);
	__rtl_setup_timeout (th, gethrtime() + interval * 1000);
	rtl_schedule();
	pthread_testcancel();

	th->resume_time = save_resume_time;
	rtl_restore_interrupts (flags);
	return 0;
}


int clock_nanosleep(rtl_clockid_t clock_id, int flags,
		const struct timespec *rqtp, struct timespec *rmtp)
{
	hrtime_t timeout;
	rtl_irqstate_t irqstate;
	hrtime_t save_resume_time;
	pthread_t self = pthread_self();
	int ret;

	if (rqtp == (const struct timespec *) &self->timeval) {
		timeout = self->timeval;
	} else {
		timeout = rtl_timespec_to_ns(rqtp);
	}

	rtl_no_interrupts (irqstate);

	if (!(flags & TIMER_ABSTIME)) {
		timeout += clock_gethrtime(CLOCK_RTL_SCHED);
	} else {
		timeout = __rtl_fix_timeout_for_clock(clock_id, timeout);
	}

	save_resume_time = self->resume_time;

	RTL_MARK_SUSPENDED(self);
	__rtl_setup_timeout (self, timeout);
	ret = rtl_schedule();
	pthread_testcancel();

	if (RTL_TIMED_OUT(&ret)) {
		self->resume_time = save_resume_time;
		rtl_restore_interrupts (irqstate);
		return 0;
	}

	/* interrupted by a signal */
	if (!(flags & TIMER_ABSTIME) && rmtp) {
		*rmtp = timespec_from_ns(self->resume_time - clock_gethrtime(CLOCK_RTL_SCHED));
	}

	self->resume_time = save_resume_time;
	rtl_restore_interrupts (irqstate);
	return EINTR;
}

int nanosleep(const struct timespec *rqtp, struct timespec *rmtp)
{
	int ret;
	if ((ret = clock_nanosleep(CLOCK_RTL_SCHED, 0, rqtp, rmtp))) {
		errno = ret;
		return -1;
	}
	return 0;
}

EXPORT_SYMBOL(pthread_delete_np);
EXPORT_SYMBOL(pthread_suspend_np);
EXPORT_SYMBOL(pthread_wakeup_np);
EXPORT_SYMBOL(pthread_make_periodic_np);
EXPORT_SYMBOL(pthread_wait_np);
EXPORT_SYMBOL(pthread_cancel);
EXPORT_SYMBOL(pthread_join);
EXPORT_SYMBOL(__pthread_create);
EXPORT_SYMBOL(rtl_sched);
