/*! Timer interrupt for edf */
static void edf_period_timer ( void *p )
{
kthread_t *kthread = p, *test;
ASSERT ( kthread );
test = kthreadq_remove ( &ksched_edf.params.edf.wait, kthread );
LOG( DEBUG, "%x %x [Period alarm]", kthread, test );
if( test == kthread )
{
if ( !edf_check_deadline ( kthread ) )
{
//LOG( DEBUG, "%x [Waked, moved to edf.ready]", kthread );
kthread_enqueue ( kthread, &ksched_edf.params.edf.ready );
if ( k_edf_schedule () )
kthreads_schedule ();
}
}
else {
/*
* thread is not in edf.wait queue, but might be running or its
* blocked - it is probable (sure?) it missed deadline
*/
LOG( DEBUG, "%x [Not in edf.wait. Missed deadline?]", kthread );
}
}
/*!
* Lock mutex object
* \param mutex Mutex descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__pthread_mutex_lock ( pthread_mutex_t *mutex )
{
kpthread_mutex_t *kmutex;
kobject_t *kobj;
int retval = EXIT_SUCCESS;
SYS_ENTRY();
ASSERT_ERRNO_AND_EXIT ( mutex, EINVAL );
kobj = mutex->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EINVAL );
kmutex = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );
retval = mutex_lock ( kmutex, kthread_get_active () );
if ( retval == 1 )
kthreads_schedule ();
/* errno is already set */
if ( retval != -1 )
SYS_RETURN ( EXIT_SUCCESS );
else
SYS_RETURN ( EXIT_FAILURE );
}
/*!
* Decrement (lock) semaphore value by 1 (if not 0 when thread is blocked)
* \param sem Semaphore descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__sem_wait ( sem_t *sem )
{
ksem_t *ksem;
kobject_t *kobj;
kthread_t *kthread;
SYS_ENTRY();
ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );
kthread = kthread_get_active ();
kobj = sem->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EINVAL );
ksem = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( ksem && ksem->id == sem->id, EINVAL );
kthread_set_errno ( kthread, EXIT_SUCCESS );
kthread_set_syscall_retval ( kthread, EXIT_SUCCESS );
if ( ksem->sem_value > 0 )
{
ksem->sem_value--;
ksem->last_lock = kthread;
}
else {
kthread_enqueue ( kthread, &ksem->queue );
kthreads_schedule ();
}
SYS_EXIT ( kthread_get_errno(NULL), kthread_get_syscall_retval(NULL) );
}
/*!
* Lock mutex object
* \param mutex Mutex descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__pthread_mutex_lock ( void *p )
{
pthread_mutex_t *mutex;
kpthread_mutex_t *kmutex;
kobject_t *kobj;
int retval = EXIT_SUCCESS;
mutex = *( (pthread_mutex_t **) p );
ASSERT_ERRNO_AND_EXIT ( mutex, EINVAL );
kobj = mutex->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EINVAL );
kmutex = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );
retval = mutex_lock ( kmutex, kthread_get_active () );
if ( retval == 1 )
kthreads_schedule ();
return retval != -1;
}
/*!
* Unlock mutex object
* \param mutex Mutex descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__pthread_mutex_unlock ( void *p )
{
pthread_mutex_t *mutex;
kpthread_mutex_t *kmutex;
kobject_t *kobj;
mutex = *( (pthread_mutex_t **) p );
ASSERT_ERRNO_AND_EXIT ( mutex, EINVAL );
kobj = mutex->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EINVAL );
kmutex = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );
if ( kmutex->owner != kthread_get_active() )
{
SET_ERRNO ( EPERM );
return EXIT_FAILURE;
}
SET_ERRNO ( EXIT_SUCCESS );
kmutex->owner = kthreadq_get ( &kmutex->queue );
if ( kmutex->owner )
{
kthreadq_release ( &kmutex->queue );
kthreads_schedule ();
}
return EXIT_SUCCESS;
}
/*!
* Decrement (lock) semaphore value by 1 (if not 0 when thread is blocked)
* \param sem Semaphore descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__sem_wait ( void *p )
{
sem_t *sem;
ksem_t *ksem;
kobject_t *kobj;
kthread_t *kthread;
sem = *( (sem_t **) p );
ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );
kthread = kthread_get_active ();
kobj = sem->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EINVAL );
ksem = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( ksem && ksem->id == sem->id, EINVAL );
kthread_set_errno ( kthread, EXIT_SUCCESS );
if ( ksem->sem_value > 0 )
{
ksem->sem_value--;
ksem->last_lock = kthread;
}
else {
kthread_enqueue ( kthread, &ksem->queue, 1, NULL, NULL );
kthreads_schedule ();
}
return EXIT_SUCCESS;
}
/*!
* Create new thread (params on user stack!)
* \param thread User level thread descriptor
* \param attr Thread attributes
* \param start_routine Starting function for new thread
* \param arg Parameter sent to starting function
* (parameters are on calling thread stack)
*/
int sys__pthread_create ( void *p )
{
pthread_t *thread;
pthread_attr_t *attr;
void *(*start_routine) (void *);
void *arg;
kthread_t *kthread;
uint flags = 0;
int sched_policy = SCHED_FIFO;
int sched_priority = THREAD_DEF_PRIO;
void *stackaddr = NULL;
size_t stacksize = 0;
thread = *( (pthread_t **) p ); p += sizeof (pthread_t *);
attr = *( (pthread_attr_t **) p ); p += sizeof (pthread_attr_t *);
start_routine = *( (void **) p ); p += sizeof (void *);
arg = *( (void **) p );
if ( attr )
{
flags = attr->flags;
sched_policy = attr->sched_policy;
sched_priority = attr->sched_params.sched_priority;
stackaddr = attr->stackaddr;
stacksize = attr->stacksize;
ASSERT_ERRNO_AND_EXIT (
sched_policy >= 0 && sched_policy < SCHED_NUM,
ENOTSUP
);
ASSERT_ERRNO_AND_EXIT (
sched_priority >= THREAD_MIN_PRIO &&
sched_priority <= THREAD_MAX_PRIO,
ENOMEM
);
/* if ( flags & SOMETHING ) change attributes ... */
}
kthread = kthread_create ( start_routine, arg, flags,
sched_policy, sched_priority,
stackaddr, stacksize );
ASSERT_ERRNO_AND_EXIT ( kthread, ENOMEM );
if ( thread )
{
thread->ptr = kthread;
thread->id = kthread_get_id (kthread);
}
SET_ERRNO ( EXIT_SUCCESS );
kthreads_schedule ();
return EXIT_SUCCESS;
}
/*! Unlock device */
int k_device_unlock ( kdevice_t *dev )
{
if ( kthreadq_release ( &dev->thrq ) )
kthreads_schedule ();
else
dev->locked = FALSE;
return 0;
}
/*! initialize thread structures and create idle thread */
void kthreads_init ()
{
extern kprog_t kprog;
int prio, i, j;
list_init ( &all_threads );
active_thread = NULL;
ksched_init ();
/* initially create 'idle thread' */
kernel_proc.proc = NULL;
kernel_proc.smap = NULL; /* use kernel pool */
kernel_proc.m.start = NULL;
kernel_proc.m.size = (size_t) 0xffffffff;
(void) kthread_create ( idle_thread, NULL, 0, SCHED_FIFO, 0, NULL,
0, &kernel_proc );
/* user_proc initialization */
user_proc.prog = &kprog;
user_proc.m.size = kprog.m->size;
user_proc.m.start = (void *) kprog.prog;
user_proc.proc = (void *) kprog.prog;
strcpy ( user_proc.name, kprog.prog->name );
user_proc.heap = user_proc.m.start + (uint) user_proc.proc->heap;
user_proc.stack = user_proc.m.start + (uint) user_proc.proc->stack;
user_proc.heap_size = (uint) user_proc.stack - (uint) user_proc.heap;
user_proc.stack_size = (uint) kprog.prog->end - (uint) user_proc.stack;
user_proc.thread_stack_size = DEFAULT_THREAD_STACK_SIZE;
user_proc.proc->p.heap_size = user_proc.heap_size;
user_proc.proc->p.stack_size = user_proc.stack_size;
user_proc.proc->p.thread_stack = user_proc.thread_stack_size;
/* initialize bitmap for threads stack management */
/* in stack area: kproc->smap_size thread stacks */
user_proc.smap_size = user_proc.stack_size / user_proc.thread_stack_size;
ASSERT ( user_proc.smap_size > 0 );
/* required number of uints for mask */
i = ( user_proc.smap_size + sizeof(uint)*8 - 1 ) / (sizeof(uint)*8);
user_proc.smap = kmalloc ( i * sizeof (uint) );
memset ( user_proc.smap, 0, i * sizeof (uint) );
for ( j = user_proc.smap_size%(sizeof(uint)*8+1); j < sizeof(uint)*8; j++ )
user_proc.smap[i-1] |= 1<<j;
prio = user_proc.proc->p.prio;
if ( !prio )
prio = THREAD_DEF_PRIO;
kthread_create ( user_proc.proc->p.init, NULL, 0, SCHED_FIFO, prio, NULL,
0, &user_proc );
kthreads_schedule ();
}
/*!
* Suspend thread until given time elapses
* \param clockid Clock to use
* \param flags Flags (TIMER_ABSTIME)
* \param request Suspend duration
* \param remain Remainder time if interrupted during suspension
* \return status
*/
int sys__clock_nanosleep ( void *p )
{
clockid_t clockid;
int flags;
timespec_t *request;
timespec_t *remain;
int retval = EXIT_SUCCESS;
kthread_t *kthread = kthread_get_active ();
ktimer_t *ktimer;
sigevent_t evp;
itimerspec_t itimer;
clockid = *( (clockid_t *) p ); p += sizeof (clockid_t);
flags = *( (int *) p ); p += sizeof (int);
request = *( (timespec_t **) p ); p += sizeof (timespec_t *);
remain = *( (timespec_t **) p );
ASSERT_ERRNO_AND_EXIT (
(clockid==CLOCK_REALTIME || clockid==CLOCK_MONOTONIC) &&
request && TIME_IS_SET(request),
EINVAL
);
/* Timers are used for "sleep" operations through steps 1-4 */
/* 1. create timer, but not arm it yet */
evp.sigev_notify = SIGEV_WAKE_THREAD;
evp.sigev_value.sival_ptr = kthread;
evp.sigev_notify_function = kclock_wake_thread;
retval += ktimer_create ( clockid, &evp, &ktimer, kthread );
ASSERT ( retval == EXIT_SUCCESS );
/* save remainder location, if provided */
ktimer->param = remain;
/* 2. suspend thread */
kthread_set_private_param ( kthread, ktimer );
retval += kthread_suspend ( kthread, kclock_interrupt_sleep, ktimer );
ASSERT ( retval == EXIT_SUCCESS );
/* 3. arm timer */
TIME_RESET ( &itimer.it_interval );
itimer.it_value = *request;
retval += ktimer_settime ( ktimer, flags, &itimer, NULL );
ASSERT ( retval == EXIT_SUCCESS );
/* 4. pick other thread as active */
kthreads_schedule ();
EXIT ( retval );
}
/*!
* Simple Round-Robin scheduler:
* - on timer tick move active into ready queue and pick next ready task
*/
static void ksched_rr_tick ( sigval_t sigval )
{
if ( sys__feature ( FEATURE_SCHED_RR, FEATURE_GET, 0 ) == 0 )
return;
kthread_t *active_thread = kthread_get_active();
if ( kthread_is_active ( active_thread ) ) {
kthread_move_to_ready ( active_thread, LAST );
kthreads_schedule ();
}
}
int kthread_set_prio ( kthread_t *kthread, int prio )
{
kthread_t *kthr = kthread;
int old_prio;
if ( !kthr )
kthr = active_thread;
old_prio = kthr->sched_priority;
/* change thread priority:
(i) if its active: change priority and move to ready
(ii) if its ready: remove from queue, change priority, put back
(iii) if its blocked: if queue is sorted by priority, same as (ii)
*/
switch ( kthr->state.state )
{
case THR_STATE_ACTIVE:
kthr->sched_priority = prio;
kthread_move_to_ready ( kthr, LAST );
kthreads_schedule ();
break;
case THR_STATE_READY:
kthread_remove_from_ready (kthr);
kthr->sched_priority = prio;
kthread_move_to_ready ( kthr, LAST );
kthreads_schedule ();
break;
case THR_STATE_WAIT: /* as now there is only FIFO queue */
kthr->sched_priority = prio;
break;
case THR_STATE_PASSIVE: /* report error or just change priority? */
kthr->sched_priority = prio;
break;
}
return old_prio;
}
/*!
* Wait for signal
* \param set Signals thread is waiting for
* \param info Where to save caught signal
* \return signal number if signal is caught,
* -1 otherwise and appropriate error number is set
*/
int sys__sigwaitinfo ( void *p )
{
sigset_t *set;
siginfo_t *info;
kthread_t *kthread;
ksignal_handling_t *sh;
ksiginfo_t *ksig, *next;
int retval;
set = *( (sigset_t **) p ); p += sizeof (sigset_t *);
info = *( (siginfo_t **) p );
ASSERT_ERRNO_AND_EXIT ( set, EINVAL );
set = U2K_GET_ADR ( set, kthread_get_process (NULL) );
ASSERT_ERRNO_AND_EXIT ( set, EINVAL );
if ( info )
info = U2K_GET_ADR ( info, kthread_get_process (NULL) );
kthread = kthread_get_active ();
sh = kthread_get_sigparams ( kthread );
/* first, search for such signal in pending signals */
ksig = list_get ( &sh->pending_signals, FIRST );
while ( ksig )
{
next = list_get_next ( &ksig->list );
if ( sigtestset ( set, ksig->siginfo.si_signo ) )
{
retval = ksig->siginfo.si_signo;
if ( info )
*info = ksig->siginfo;
list_remove ( &sh->pending_signals, 0, &ksig->list );
kfree ( ksig );
EXIT2 ( EXIT_SUCCESS, retval );
}
ksig = next;
}
/*
* if no pending signal found that matches given mask
* suspend thread until signal is received
*/
kthread_suspend ( kthread, ksignal_received_signal, NULL );
kthreads_schedule ();
EXIT ( EINTR ); /* if other events wake thread */
}
static int cond_release ( void *p, int release_all )
{
pthread_cond_t *cond;
kpthread_cond_t *kcond;
kpthread_mutex_t *kmutex;
kobject_t *kobj_cond, *kobj_mutex;
kthread_t *kthread;
int retval = 0;
cond = *( (pthread_cond_t **) p );
ASSERT_ERRNO_AND_EXIT ( cond, EINVAL );
kobj_cond = cond->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj_cond, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj_cond->list ),
EINVAL );
kcond = kobj_cond->kobject;
ASSERT_ERRNO_AND_EXIT ( kcond && kcond->id == cond->id, EINVAL );
kthread_set_errno ( kthread_get_active (), EXIT_SUCCESS );
if ( (kthread = kthreadq_remove ( &kcond->queue, NULL )) )
{
kobj_mutex = kthread_get_private_param ( kthread );
kmutex = kobj_mutex->kobject;
retval = mutex_lock ( kmutex, kthread );
if ( retval == 0 )
kthread_move_to_ready ( kthread, LAST );
/* process other threads in queue */
while ( release_all &&
(kthread = kthreadq_remove ( &kcond->queue, NULL )) )
{
kthread_set_errno ( kthread, EXIT_SUCCESS );
kobj_mutex = kthread_get_private_param ( kthread );
kmutex = kobj_mutex->kobject;
kthread_enqueue(kthread, &kmutex->queue, 0, NULL, NULL);
}
}
if ( retval > -1 )
kthreads_schedule ();
return EXIT_SUCCESS;
}
/*! Lock device */
int k_device_lock ( kdevice_t *dev, int wait )
{
if ( !wait && dev->locked )
return -1;
if ( dev->locked )
{
kthread_enqueue ( NULL, &dev->thrq );
kthreads_schedule ();
}
dev->locked = TRUE;
return 0;
}
/*!
* Create new thread (params on user stack!)
* \param thread User level thread descriptor
* \param attr Thread attributes
* \param start_routine Starting function for new thread
* \param arg Parameter sent to starting function
* (parameters are on calling thread stack)
*/
int sys__pthread_create ( pthread_t *thread, pthread_attr_t *attr,
void *(*start_routine) (void *), void *arg )
{
kthread_t *kthread;
uint flags = 0;
int sched_policy = SCHED_FIFO;
int sched_priority = THREAD_DEF_PRIO;
void *stackaddr = NULL;
size_t stacksize = 0;
SYS_ENTRY();
if ( attr )
{
flags = attr->flags;
sched_policy = attr->sched_policy;
sched_priority = attr->sched_params.sched_priority;
stackaddr = attr->stackaddr;
stacksize = attr->stacksize;
ASSERT_ERRNO_AND_EXIT ( sched_policy == SCHED_FIFO,
ENOTSUP );
ASSERT_ERRNO_AND_EXIT (
sched_priority >= THREAD_MIN_PRIO &&
sched_priority <= THREAD_MAX_PRIO,
ENOMEM
);
/* if ( flags & SOMETHING ) change attributes ... */
}
kthread = kthread_create ( start_routine, arg, flags,
sched_policy, sched_priority,
stackaddr, stacksize );
ASSERT_ERRNO_AND_EXIT ( kthread, ENOMEM );
if ( thread )
{
thread->ptr = kthread;
thread->id = kthread_get_id (kthread);
}
kthreads_schedule ();
SYS_EXIT ( EXIT_SUCCESS, EXIT_SUCCESS );
}
/*!
* Wait on conditional variable
* \param cond conditional variable descriptor (user level descriptor)
* \param mutex Mutex descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__pthread_cond_wait ( void *p )
{
pthread_cond_t *cond;
pthread_mutex_t *mutex;
kpthread_cond_t *kcond;
kpthread_mutex_t *kmutex;
kobject_t *kobj_cond, *kobj_mutex;
int retval = EXIT_SUCCESS;
cond = *( (pthread_cond_t **) p ); p += sizeof (pthread_cond_t *);
mutex = *( (pthread_mutex_t **) p );
ASSERT_ERRNO_AND_EXIT ( cond && mutex, EINVAL );
kobj_cond = cond->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj_cond, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj_cond->list ),
EINVAL );
kcond = kobj_cond->kobject;
ASSERT_ERRNO_AND_EXIT ( kcond && kcond->id == cond->id, EINVAL );
kobj_mutex = mutex->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj_mutex, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj_mutex->list),
EINVAL );
kmutex = kobj_mutex->kobject;
ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );
ASSERT_ERRNO_AND_EXIT ( kmutex->owner == kthread_get_active(), EPERM );
SET_ERRNO ( EXIT_SUCCESS );
/* move thread in conditional variable queue */
kthread_enqueue ( NULL, &kcond->queue, 0, NULL, NULL );
/* save reference to mutex object */
kthread_set_private_param ( NULL, kobj_mutex );
/* release mutex */
kmutex->owner = kthreadq_get ( &kmutex->queue );
if ( kmutex->owner )
kthreadq_release ( &kmutex->queue );
kthreads_schedule ();
return retval;
}
/*!
* Wait on conditional variable
* \param cond conditional variable descriptor (user level descriptor)
* \param mutex Mutex descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__pthread_cond_wait ( pthread_cond_t *cond, pthread_mutex_t *mutex )
{
kpthread_cond_t *kcond;
kpthread_mutex_t *kmutex;
kobject_t *kobj_cond, *kobj_mutex;
SYS_ENTRY();
ASSERT_ERRNO_AND_EXIT ( cond && mutex, EINVAL );
kobj_cond = cond->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj_cond, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj_cond->list ),
EINVAL );
kcond = kobj_cond->kobject;
ASSERT_ERRNO_AND_EXIT ( kcond && kcond->id == cond->id, EINVAL );
kobj_mutex = mutex->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj_mutex, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj_mutex->list),
EINVAL );
kmutex = kobj_mutex->kobject;
ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );
ASSERT_ERRNO_AND_EXIT ( kmutex->owner == kthread_get_active(), EPERM );
kthread_set_errno ( NULL, EXIT_SUCCESS );
kthread_set_syscall_retval ( NULL, EXIT_SUCCESS );
/* move thread in conditional variable queue */
kthread_enqueue ( NULL, &kcond->queue );
/* save reference to mutex object */
kthread_set_private_param ( NULL, kobj_mutex );
/* release mutex */
kmutex->owner = kthreadq_get ( &kmutex->queue );
if ( kmutex->owner )
kthreadq_release ( &kmutex->queue );
kthreads_schedule ();
SYS_EXIT ( kthread_get_errno(NULL), kthread_get_syscall_retval(NULL) );
}
/*! initialize thread structures and create idle thread */
void kthreads_init ()
{
list_init ( &all_threads );
list_init ( &procs );
active_thread = NULL;
ksched_init ();
/* initially create 'idle thread' */
kernel_proc.prog = NULL;
kernel_proc.stack_pool = NULL; /* use kernel pool */
kernel_proc.m.start = NULL;
kernel_proc.m.size = (size_t) 0xffffffff;
(void) kthread_create ( idle_thread, NULL, 0, SCHED_FIFO, 0, NULL,
NULL, 0, &kernel_proc );
kthreads_schedule ();
}
/*!
* Wait for thread termination
* \param thread Thread descriptor (user level descriptor)
* \param retval Where to store exit status of joined thread
* \return 0 if thread already gone; -1 if not finished and 'wait' not set;
* 'thread exit status' otherwise
*/
int sys__pthread_join ( void *p )
{
pthread_t *thread;
void **retval;
kthread_t *kthread;
int ret_value = 0;
thread = *( (pthread_t **) p ); p += sizeof (pthread_t *);
retval = *( (void ***) p );
ASSERT_ERRNO_AND_EXIT ( thread, ESRCH );
kthread = thread->ptr;
if ( kthread_get_id (kthread) != thread->id )
{
/* at 'kthread' is now something else */
ret_value = EXIT_FAILURE;
SET_ERRNO ( ESRCH );
}
else if ( kthread_is_alive (kthread) )
{
ret_value = EXIT_SUCCESS;
SET_ERRNO ( EXIT_SUCCESS );
kthread_set_private_param ( kthread_get_active(), retval );
kthread_wait_thread ( NULL, kthread );
kthreads_schedule ();
}
else {
/* target thread is passive, collect status and free descr. */
ret_value = EXIT_SUCCESS;
SET_ERRNO ( EXIT_SUCCESS );
kthread_collect_status ( kthread, retval );
}
return ret_value;
}
/*! Timer interrupt for Round Robin */
static void rr_timer ( void *p )
{
kthread_t *kthread = p;
kthread_sched_data_t *tsched;
if ( kthread_get_active () != kthread )
{
/* bug or rr thread got canceled! Let asume second :) */
return;
}
tsched = kthread_get_sched_param ( kthread );
/* given time is elapsed, set remainder to zero */
tsched->params.rr.remainder.sec = tsched->params.rr.remainder.nsec = 0;
/* move thread to ready queue - as last in coresponding queue */
kthread_move_to_ready ( kthread, LAST );
kthreads_schedule ();
}
/*!
* Increment (lock) semaphore value by 1 (or unblock one thread that is blocked)
* \param sem Semaphore descriptor (user level descriptor)
* \return 0 if successful, -1 otherwise and appropriate error number is set
*/
int sys__sem_post ( void *p )
{
sem_t *sem;
ksem_t *ksem;
kobject_t *kobj;
kthread_t *kthread, *released;
sem = *( (sem_t **) p );
ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );
kthread = kthread_get_active ();
kobj = sem->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EINVAL );
ksem = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( ksem && ksem->id == sem->id, EINVAL );
kthread_set_errno ( kthread, EXIT_SUCCESS );
released = kthreadq_get ( &ksem->queue ); /* first to release */
if ( !released || ksem->sem_value < 0 )
{
/* if initial semaphore value (set by sem_init) was negative,
* semaphore will not release threads until until its value
* reaches zero (small extension of POSIX semaphore) */
ksem->sem_value++;
}
else {
kthreadq_release ( &ksem->queue );
kthreads_schedule ();
}
return EXIT_SUCCESS;
}
/*! initialize thread structures and create idle thread */
void kthreads_init ()
{
extern kprog_t prog;
int prio;
list_init ( &all_threads );
active_thread = NULL;
ksched_init ();
/* initially create 'idle thread' */
kernel_proc.prog = NULL;
kernel_proc.stack_pool = NULL; /* use kernel pool */
kernel_proc.m.start = NULL;
kernel_proc.m.size = (size_t) 0xffffffff;
(void) kthread_create ( idle_thread, NULL, 0, SCHED_FIFO, 0, NULL,
NULL, 0, &kernel_proc );
/* user_proc */
user_proc.prog = &prog;
user_proc.m.size = prog.m->size;
user_proc.m.start = user_proc.pi = prog.pi;
/* initialize memory pool for threads stacks */
user_proc.stack_pool = ffs_init (
U2K_GET_ADR ( user_proc.pi->stack, &user_proc ),
(size_t) prog.pi->end_adr - (size_t) prog.pi->stack + 1 );
prio = prog.pi->prio;
if ( !prio )
prio = THREAD_DEF_PRIO;
kthread_create ( user_proc.pi->init, NULL, 0, SCHED_FIFO, prio, NULL,
NULL, 0, &user_proc );
kthreads_schedule ();
}
/*!
* Resume suspended thread (called on timer activation)
* \param sigval Thread that should be released
*/
static void kclock_wake_thread ( sigval_t sigval )
{
kthread_t *kthread;
ktimer_t *ktimer;
int retval = 0;
kthread = sigval.sival_ptr;
ASSERT ( kthread );
ASSERT ( kthread_check_kthread ( kthread ) ); /* is this valid thread */
ASSERT ( kthread_is_suspended ( kthread, NULL, NULL ) );
ktimer = kthread_get_private_param ( kthread );
timespec_t *remain = ktimer->param;
if ( remain )
TIME_RESET ( remain ); /* timer expired */
kthread_move_to_ready ( kthread, LAST );
retval += ktimer_delete ( ktimer );
ASSERT ( retval == EXIT_SUCCESS );
kthreads_schedule ();
}
/*! initialize thread structures and create idle thread */
void kthreads_init ()
{
int prio;
list_init ( &all_threads );
active_thread = NULL;
ksched_init ();
(void) kthread_create(idle_thread,NULL,0,SCHED_FIFO,0,NULL,NULL,0);
/* initialize memory pool for threads */
pi.heap = kmalloc ( PROG_HEAP_SIZE );
pi.heap_size = PROG_HEAP_SIZE;
prio = pi.prio;
if ( !prio )
prio = THREAD_DEF_PRIO;
(void) kthread_create(pi.init,NULL,0,SCHED_FIFO,prio,NULL,NULL,0);
kthreads_schedule ();
}
/*!
* Wait for thread termination
* \param thread Thread descriptor (user level descriptor)
* \param retval Where to store exit status of joined thread
* \return 0 if thread already gone; -1 if not finished and 'wait' not set;
* 'thread exit status' otherwise
*/
int sys__pthread_join ( pthread_t *thread, void **retval )
{
kthread_t *kthread;
SYS_ENTRY();
ASSERT_ERRNO_AND_EXIT ( thread, ESRCH );
kthread = thread->ptr;
if ( kthread_get_id (kthread) != thread->id )
{
/* at 'kthread' is now something else */
SYS_EXIT ( ESRCH, EXIT_FAILURE );
}
else if ( kthread_is_alive (kthread) )
{
kthread_set_errno ( NULL, EXIT_SUCCESS );
kthread_set_syscall_retval ( NULL, EXIT_SUCCESS );
kthread_set_private_param ( kthread_get_active(), retval );
kthread_wait_thread ( NULL, kthread );
kthreads_schedule ();
SYS_EXIT ( kthread_get_errno(NULL),
kthread_get_syscall_retval(NULL) );
}
else {
/* target thread is passive, collect status and free descr. */
kthread_collect_status ( kthread, retval );
SYS_EXIT ( EXIT_SUCCESS, EXIT_SUCCESS );
}
}
/*! Activate timers and reschedule threads if required */
static void ktimer_schedule ()
{
ktimer_t *first;
timespec_t time, ref_time;
int resched = 0;
kclock_gettime ( CLOCK_REALTIME, &time );
/* should have separate "scheduler" for each clock */
ref_time = time;
time_add ( &ref_time, &threshold );
/* use "ref_time" instead of "time" when looking timers to activate */
/* should any timer be activated? */
first = list_get ( &ktimers, FIRST );
while ( first != NULL )
{
/* timers have absolute values in 'it_value' */
if ( time_cmp ( &first->itimer.it_value, &ref_time ) <= 0 )
{
/* 'activate' timer */
/* but first remove timer from list */
first = list_remove ( &ktimers, FIRST, NULL );
/* and add to list if period is given */
if ( TIME_IS_SET ( &first->itimer.it_interval) )
{
/* calculate next activation time */
time_add ( &first->itimer.it_value,
&first->itimer.it_interval );
/* put back into list */
list_sort_add ( &ktimers, first,
&first->list, ktimer_cmp );
}
else {
TIMER_DISARM ( first );
}
if ( first->owner == NULL )
{
/* timer set by kernel - call now, directly */
if ( first->evp.sigev_notify_function )
first->evp.sigev_notify_function (
first->evp.sigev_value
);
}
else {
/* timer set by thread */
if ( !ksignal_process_event (
&first->evp, first->owner, SI_TIMER ) )
{
resched++;
}
}
first = list_get ( &ktimers, FIRST );
}
else {
break;
}
}
first = list_get ( &ktimers, FIRST );
if ( first )
{
ref_time = first->itimer.it_value;
time_sub ( &ref_time, &time );
arch_timer_set ( &ref_time, ktimer_schedule );
}
if ( resched )
kthreads_schedule ();
}
static int kmq_receive ( void *p, kthread_t *receiver )
{
mqd_t *mqdes;
char *msg_ptr;
size_t msg_len;
uint *msg_prio;
kmq_queue_t *kq_queue;
kobject_t *kobj;
kmq_msg_t *kmq_msg;
kthread_t *kthread;
int retval;
mqdes = *( (mqd_t **) p ); p += sizeof (mqd_t *);
msg_ptr = *( (char **) p ); p += sizeof (char *);
msg_len = *( (size_t *) p ); p += sizeof (size_t);
msg_prio = *( (uint **) p );
ASSERT_ERRNO_AND_EXIT ( mqdes && msg_ptr, -EINVAL );
kobj = mqdes->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, -EBADF );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
-EBADF );
kq_queue = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( kq_queue->id == mqdes->id, -EBADF );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kmq_queue, &kq_queue->list ),
-EBADF );
if ( kq_queue->attr.mq_curmsgs == 0 )
{
if ( (kobj->flags & O_NONBLOCK) )
return -EAGAIN;
/* block thread */
kthread_enqueue ( receiver, &kq_queue->recv_q, 1, NULL, NULL );
kthreads_schedule ();
return -EAGAIN;
}
if ( msg_len < kq_queue->attr.mq_msgsize )
return -EMSGSIZE;
kmq_msg = list_remove ( &kq_queue->msg_list, FIRST, NULL );
memcpy ( msg_ptr, &kmq_msg->msg_data[0], kmq_msg->msg_size );
msg_len = kmq_msg->msg_size;
if ( msg_prio )
*msg_prio = kmq_msg->msg_prio;
kfree (kmq_msg);
kq_queue->attr.mq_curmsgs--;
/* is there a blocked sender? */
if ( (kthread = kthreadq_remove ( &kq_queue->send_q, NULL )) )
{
/* "save" receiver thread */
kthread_move_to_ready ( receiver, FIRST );
kthread_set_errno ( receiver, EXIT_SUCCESS );
kthread_set_syscall_retval ( receiver, msg_len );
/* unblock sender */
kthread_set_active ( kthread ); /* temporary */
p = arch_syscall_get_params ( kthread_get_context (kthread) );
retval = kmq_send ( p, kthread );
if ( retval == EXIT_SUCCESS )
{
kthread_set_errno ( kthread, EXIT_SUCCESS );
kthread_set_syscall_retval ( kthread, retval );
}
else {
kthread_set_errno ( kthread, retval );
kthread_set_syscall_retval ( kthread, EXIT_FAILURE );
}
kthreads_schedule ();
}
return msg_len;
}
static int kmq_send ( void *p, kthread_t *sender )
{
mqd_t *mqdes;
char *msg_ptr;
size_t msg_len;
uint msg_prio;
kmq_queue_t *kq_queue;
kobject_t *kobj;
kmq_msg_t *kmq_msg;
kthread_t *kthread;
int retval;
mqdes = *( (mqd_t **) p ); p += sizeof (mqd_t *);
msg_ptr = *( (char **) p ); p += sizeof (char *);
msg_len = *( (size_t *) p ); p += sizeof (size_t);
msg_prio = *( (uint *) p );
ASSERT_ERRNO_AND_EXIT ( mqdes && msg_ptr, EINVAL );
ASSERT_ERRNO_AND_EXIT ( msg_prio <= MQ_PRIO_MAX, EINVAL );
kobj = mqdes->ptr;
ASSERT_ERRNO_AND_EXIT ( kobj, EBADF );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
EBADF );
kq_queue = kobj->kobject;
ASSERT_ERRNO_AND_EXIT ( kq_queue->id == mqdes->id, EBADF );
ASSERT_ERRNO_AND_EXIT ( list_find ( &kmq_queue, &kq_queue->list ),
EBADF );
if ( kq_queue->attr.mq_curmsgs >= kq_queue->attr.mq_maxmsg )
{
if ( (kobj->flags & O_NONBLOCK) )
return EAGAIN;
/* block thread */
kthread_enqueue ( sender, &kq_queue->send_q, 1, NULL, NULL );
kthreads_schedule ();
return EAGAIN;
}
if ( msg_len > kq_queue->attr.mq_msgsize )
return EMSGSIZE;
kmq_msg = kmalloc ( sizeof (kmq_msg_t) + msg_len );
ASSERT_ERRNO_AND_EXIT ( kmq_msg, ENOMEM );
/* create message */
kmq_msg->msg_size = msg_len;
kmq_msg->msg_prio = msg_prio;
memcpy ( &kmq_msg->msg_data[0], msg_ptr, msg_len );
list_sort_add ( &kq_queue->msg_list, kmq_msg, &kmq_msg->list,
( int (*)(void *, void *) ) cmp_mq_msg );
kq_queue->attr.mq_curmsgs++;
/* is there a blocked receiver? */
if ( (kthread = kthreadq_remove ( &kq_queue->recv_q, NULL )) )
{
/* "save" sender thread */
kthread_move_to_ready ( sender, FIRST );
kthread_set_errno ( sender, EXIT_SUCCESS );
kthread_set_syscall_retval ( sender, EXIT_SUCCESS );
/* unblock receiver */
kthread_set_active ( kthread ); /* temporary */
p = arch_syscall_get_params ( kthread_get_context (kthread) );
retval = kmq_receive ( p, kthread );
if ( retval >= 0 )
{
kthread_set_errno ( kthread, EXIT_SUCCESS );
kthread_set_syscall_retval ( kthread, retval );
}
else {
kthread_set_errno ( kthread, -retval );
kthread_set_syscall_retval ( kthread, EXIT_FAILURE );
}
kthreads_schedule ();
}
return EXIT_SUCCESS;
}
/*!
* Cancel thread (or restore it to previous state)
* \param kthread Thread descriptor
*/
int kthread_exit ( kthread_t *kthread, void *exit_status, int force )
{
kthread_state_t *prev;
int restored = FALSE;
kthread_t *released;
kthread_q *q;
void **p;
ASSERT ( kthread );
do {
prev = list_get ( &kthread->states, FIRST );
if ( prev )
restored = kthread_restore_state ( kthread );
}
while ( prev && force ); /* if cancel is called, destroy all states */
if ( restored && !force ) /* restored to previous state */
{
if ( kthread == active_thread )
kthread->state.state = THR_STATE_ACTIVE;
kthreads_schedule ();
return EXIT_SUCCESS;
}
if ( kthread->state.state == THR_STATE_PASSIVE )
return EXIT_SUCCESS; /* thread is already finished */
if ( kthread->state.state == THR_STATE_READY )
{
/* remove target 'thread' from its queue */
if ( !kthread_remove_from_ready ( kthread ) )
ASSERT ( FALSE );
}
else if ( kthread->state.state == THR_STATE_WAIT )
{
/* remove target 'thread' from its queue */
if ( !kthreadq_remove ( kthread->queue, kthread ) )
ASSERT ( FALSE );
}
else if ( kthread->state.state == THR_STATE_SUSPENDED )
{
/* cancellation routine */
if ( kthread->state.cancel_suspend_handler )
kthread->state.cancel_suspend_handler ( kthread,
kthread->state.cancel_suspend_param );
}
else if ( kthread->state.state == THR_STATE_ACTIVE )
{
if ( kthread != active_thread )
return ESRCH; /* thread descriptor corrupted ! */
}
else {
return ESRCH; /* thread descriptor corrupted ! */
}
kthread->state.state = THR_STATE_PASSIVE;
kthread->ref_cnt--;
kthread->state.exit_status = exit_status;
kthread->proc->thread_count--;
arch_destroy_thread_context ( &kthread->state.context );
kthread_restore_state ( kthread );
q = &kthread->join_queue;
while ( (released = kthreadq_remove ( q, NULL )) != NULL )
{
/* save exit status to all waiting threads */
p = kthread_get_private_param ( released );
if ( p )
*p = exit_status;
kthread_move_to_ready ( released, LAST );
kthread->ref_cnt--;
}
if ( !kthread->ref_cnt )
kthread_remove_descriptor ( kthread );
if ( kthread == active_thread )
{
active_thread = NULL;
kthreads_schedule ();
}
return EXIT_SUCCESS;
}