/* 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 * */ /* for tracing tasks scheduling events */ /* J. Vidal */ #define __SCHEDULER__ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /* for tracing tasks scheduling events */ /* J. Vidal */ #include #ifdef _RTL_POSIX_TIMERS #include #endif 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) */ 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 (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, &thread->pending); #ifdef _RTL_POSIX_SIGNALS if ((signal >= RTL_SIGUSR1) && HIGHER_PRIORITY_THREAD(thread,pthread_self())) rtl_schedule(); #endif 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; } static void rtl_task_free_memory(void *p) { struct rtl_thread_struct *task = (struct rtl_thread_struct *) p; 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; struct tq_struct free_task; rtl_irqstate_t interrupt_state; rtl_no_interrupts (interrupt_state); memset(&free_task, 0, sizeof(free_task)); free_task.data = thread; free_task.routine = rtl_task_free_memory; thread->cleanup = (struct rtl_cleanup_struct *) &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(); #ifdef _RTL_POSIX_SIGNALS /* Really necessary to block user signals?*/ rtl_sigset_t mask; clear_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&self->pending); rtl_sigfillset(&mask); pthread_sigmask(SIG_SETMASK,&mask,NULL); #endif rtl_no_interrupts(flags); pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL); do_cleanups(self); rtl_posix_cleanup(retval); set_bit (RTL_THREAD_FINISHED, &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); #ifdef __CPU_TRACER rt_print_request(p, 0); #endif 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, timespec_to_ns (&value->it_value), timespec_to_ns (&value->it_interval)); } return 0; } inline static hrtime_t find_preemptor(schedule_t *s, struct rtl_thread_struct *chosen){ struct rtl_thread_struct *t; struct rtl_thread_struct *preemptor=0; #ifdef _RTL_POSIX_TIMERS timer_t timer =0; hrtime_t preempt_time=HRTIME_INFINITY; #endif for (t = s->rtl_tasks; t; t = t->next) { if (test_bit(RTL_THREAD_TIMERARMED, &t->threadflags)) { if (t->sched_param.sched_priority > chosen->sched_param.sched_priority) { if(!preemptor ||(t->resume_time < preemptor->resume_time)) { preemptor = t; } } } } #ifdef _RTL_POSIX_TIMERS if (preemptor) preempt_time=preemptor->resume_time; for (timer=get_timer_list_start();timer;timer=timer->next){ // find preemptor. if (timer->owner->sched_param.sched_priority > chosen->sched_param.sched_priority){ if( TIMER_ARMED(timer)){ if (timer->expires.it_value < preempt_time) { preempt_time = timer->expires.it_value; } } } else { /* timer list is ordered by owner priority */ break; } } return (preempt_time!=HRTIME_INFINITY)? preempt_time : 0 ; #endif return (preemptor)? preemptor->resume_time : 0 ; } #define do_abort(t) do { clear_bit(RTL_THREAD_TIMERARMED, &t->threadflags); if (t->abort) { t->abort(t->abortdata); }} while (0) static void rtl_posix_cancel(int signal) { set_bit (RTL_CANCELPENDING, &pthread_self()->threadflags); if (test_bit (RTL_CANCELTYPE, &pthread_self()->threadflags)) { pthread_testcancel(); } } static void (*rtl_sigcancel_handler)(int signal) = rtl_posix_cancel; static inline void do_signal(pthread_t t) { rtl_irqstate_t flags; rtl_no_interrupts(flags); if (test_and_clear_bit(RTL_SIGNAL_SUSPEND, &t->pending)) { /*t->abort = 0; t->abortdata = 0;*/ do_abort(t); RTL_MARK_SUSPENDED(t); } if (test_and_clear_bit(RTL_SIGNAL_WAKEUP, &t->pending)) { RTL_MARK_READY(t); do_abort(t); } if (test_and_clear_bit(RTL_SIGNAL_TIMER, &t->pending)) { RTL_MARK_READY(t); do_abort(t); } if (!test_bit(RTL_SIGNAL_CANCEL, &t->blocked) && test_and_clear_bit(RTL_SIGNAL_CANCEL, &t->pending)) { RTL_MARK_READY(t); do_abort(t); rtl_restore_interrupts(flags); (*rtl_sigcancel_handler)(RTL_SIGNAL_CANCEL); } rtl_restore_interrupts(flags); } #ifdef _RTL_POSIX_SIGNALS static inline void do_user_signal(pthread_t t){ int sig; unsigned long flags, initial_interrupt_state; void (*fun)(int)=NULL; rtl_no_interrupts(initial_interrupt_state); for (sig=RTL_MAX_SIGNAL;sig>=RTL_SIGUSR1;sig--){ if (test_bit(sig,&t->pending) && !test_bit(sig,&t->blocked)){ if (rtl_sigact[sig].owner==t){ fun=rtl_sigact[sig].sa_handler; if (fun) { // Block signal being managed. set_bit(sig,&t->blocked); set_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&t->pending); rtl_allow_interrupts(); fun(sig); rtl_no_interrupts(flags); clear_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&t->pending); clear_bit(sig,&t->blocked); if (test_and_clear_bit(RTL_THREAD_SIGNAL_INTERRUMPIBLE,&t->threadflags)){ pthread_kill(t,RTL_SIGNAL_WAKEUP); } } clear_bit (RTL_SCHED_TIMER_OK, &LOCAL_SCHED->sched_flags); } /* The bit should be pending until unblocked. */ clear_bit(sig,&t->pending); } } rtl_restore_interrupts(initial_interrupt_state); } #endif int rtl_schedule (void) { schedule_t *sched; struct rtl_thread_struct *t; struct rtl_thread_struct *new_task; hrtime_t preempt_time = 0; unsigned long interrupt_state; int cpu_id = rtl_getcpuid(); hrtime_t now; rtl_sigset_t mask; #ifdef _RTL_POSIX_TIMERS timer_t timer = 0; #endif idle: 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); #ifdef __CPU_TRACER rt_print_exec(sched, now); ini_time=now; #endif if (sched->clock->mode == RTL_CLOCK_MODE_ONESHOT) { sched->clock->value = now; } #ifdef _RTL_POSIX_TIMERS for (timer=get_timer_list_start();timer;timer=timer->next){ if (timer_expiration(timer,now,sched->clock->mode)){ expiration_notification(timer); UPDATE_TIMER_IN_DEBUG_MODE(timer); } } #endif for (t = sched->rtl_tasks; t; t = t->next) { /* expire timers */ if (test_bit(RTL_THREAD_TIMERARMED, &t->threadflags)) { if (now >= t->resume_time) { clear_bit(RTL_THREAD_TIMERARMED, &t->threadflags); rtl_sigaddset (&t->pending, RTL_SIGNAL_TIMER); if (t->period != 0) { /* periodic */ t->resume_time += t->period; #ifdef __CPU_TRACER rt_print_request(t, (t == &sched->rtl_linux_task) ); #endif /* 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 (!new_task) { rtl_allow_interrupts(); /* rtl_cprintf ("idle"); */ goto idle; } if (sched->clock->mode == RTL_CLOCK_MODE_ONESHOT && !test_bit (RTL_SCHED_TIMER_OK, &sched->sched_flags)) { if ( (preempt_time = find_preemptor(sched,new_task)) ){ (sched->clock)->settimer(sched->clock, preempt_time - now); } else { (sched->clock)->settimer(sched->clock, (HRTICKS_PER_SEC / HZ) / 2); } set_bit (RTL_SCHED_TIMER_OK, &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; rtl_restore_interrupts(interrupt_state); if (pthread_self()->pending & ~(1 << RTL_SIGNAL_READY)) do_signal(pthread_self()); #ifdef _RTL_POSIX_SIGNALS /* Signal handlers are non re-entrant, i.e. first finish current handler execution and then manage the rest of pending signals. */ if (test_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&pthread_self()->pending)) goto end; if ((pthread_self()->pending & ~pthread_self()->blocked) & RTL_THREAD_SIGNALS_MASK) do_user_signal(pthread_self()); #endif if (!rtl_sigismember(&pthread_self()->pending, RTL_SIGNAL_READY)) { goto idle; } end: rtl_trace2 (RTL_TRACE_SCHED_OUT, (long) pthread_self()); return mask; } int rtl_cpu_exists (int cpu) { int n; int i; for (i = 0; i < rtl_num_cpus(); i++) { n = cpu_logical_map (i); if (n == cpu) { return 1; } } return 0; } 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) { #ifdef CONFIG_SMP 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; task -> threadflags = 0; if (attr->detachstate == PTHREAD_CREATE_JOINABLE) { set_bit(RTL_THREAD_JOINABLE, &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; #ifdef __CPU_TRACER task->user[0]=(int *)++ntasks; #endif { 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, &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, &pthread_self()->blocked); } else { clear_bit (RTL_SIGNAL_CANCEL, &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, &pthread_self()->threadflags); } if (type) { set_bit (RTL_CANCELTYPE, &pthread_self()->threadflags); } else { clear_bit (RTL_CANCELTYPE, &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, &pthread_self()->threadflags)) { pthread_exit(PTHREAD_CANCELED); } } int pthread_delete_np (pthread_t thread) { hrtime_t timeout = gethrtime() + 1000000000; clear_bit (RTL_SIGNAL_CANCEL, &thread->blocked); set_bit (RTL_CANCELTYPE, &thread->threadflags); set_bit(RTL_CANCELPENDING, &thread->threadflags); clear_bit(RTL_THREAD_JOINABLE, &thread->threadflags); /* now the cancel is enabled+asynchronous */ pthread_cancel (thread); while (!test_bit (RTL_THREAD_FINISHED, &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 */ static void 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 tq_struct *) th->cleanup); } rtl_spin_lock_irqsave(&rtl_tqueue_lock, flags); } rtl_spin_unlock_irqrestore(&rtl_tqueue_lock, flags); } 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; int ret; int my_cpu_id; schedule_t *s; unsigned int cpu_id = rtl_getcpuid(); #ifdef __CPU_TRACER int fifo; #endif #ifdef _RTL_POSIX_TIMERS rtl_spin_lock_init (&rtl_timer_list_lock); #endif 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; for (i = 0; i < rtl_num_cpus(); i++) { 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; #ifdef CONFIG_SMP for (i = 0; i < rtl_num_cpus(); i++) { int cpu; int ret; cpu = cpu_logical_map (i); s = &rtl_sched [cpu]; ret = rtl_request_ipi(resched_irq, cpu); } #endif rtl_restore_interrupts (interrupt_state); save_errno_location = __errno_location; __errno_location = thread_errno_location; #ifdef __CPU_TRACER rtf_destroy (FIFO); fifo = rtf_create(FIFO, 32768); #endif /* rtl_setdebug (RTLDBG_ALL); */ return 0; } void cleanup_module(void) { int i; 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); #ifdef CONFIG_SMP rtl_free_ipi(cpu); #endif } #ifdef __CPU_TRACER rtf_destroy (FIFO); #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() + (long long) interval * 1000LL); rtl_schedule(); pthread_testcancel(); th->resume_time = save_resume_time; rtl_restore_interrupts (flags); return 0; } int clock_nanosleep(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 = 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; #ifdef _RTL_POSIX_SIGNALS set_bit(RTL_THREAD_SIGNAL_INTERRUMPIBLE,&self->threadflags); #endif RTL_MARK_SUSPENDED(self); __rtl_setup_timeout (self, timeout); ret = rtl_schedule(); #ifdef _RTL_POSIX_SIGNALS clear_bit(RTL_THREAD_SIGNAL_INTERRUMPIBLE,&self->threadflags); #endif 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; }