/*! lock mutex; return 0 if locked, 1 if thread blocked, -1 if error */
static int mutex_lock ( kpthread_mutex_t *kmutex, kthread_t *kthread )
{
if ( !kmutex->owner )
{
/* mutex was not locked, acquire lock on it */
kmutex->owner = kthread;
kthread_set_errno ( kthread, EXIT_SUCCESS );
return 0;
}
else {
/* mutex was locked */
/* recursive locking? */
if ( kmutex->owner == kthread )
{
kthread_set_errno ( kthread, EDEADLK );
return -1;
}
kthread_set_errno ( kthread, EXIT_SUCCESS );
kthread_enqueue ( kthread, &kmutex->queue, 0, NULL, NULL );
return 1;
}
}
/*!
* Put given thread or active thread (when kthread == NULL) into queue of
* "waited" thread
*/
void kthread_wait_thread ( kthread_t *waiting, kthread_t *waited )
{
ASSERT ( waited );
waited->ref_cnt++;
kthread_enqueue ( waiting, &waited->join_queue, 0, NULL, NULL );
}
/*!
* 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) );
}
/*! Timer interrupt for edf */
static void edf_period_alarm ( sigval_t sigev_value )
{
kthread_t *kthread = sigev_value.sival_ptr, *test;
ksched_t *ksched;
ASSERT ( kthread );
ksched = ksched2_get ( kthread_get_sched_policy (kthread) );
test = kthreadq_remove ( &ksched->params.edf.wait, kthread );
EDF_LOG ( "%x %x [Period alarm]", kthread, test );
if( test == kthread )
{
if ( !edf_check_deadline ( kthread ) )
{
EDF_LOG ( "%x [Waked, moved to edf.ready]", kthread );
kthread_enqueue ( kthread, &ksched->params.edf.ready );
edf_schedule (ksched);
}
/* else => missed deadline -- handle with deadline timer */
}
else {
/*
* thread is not in edf.wait queue, but might be running or its
* blocked - it is probable it missed its deadline, but that
* will be handled with different timer
*/
EDF_LOG ( "%x [Not in edf.wait. Missed deadline?]", kthread );
}
}
/*!
* 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;
}
/*! 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 );
}
}
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;
}
/*!
* 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) );
}
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 void edf_deadline_timer ( void *p )
{
alarm_t alarm;
kthread_t *kthread = p, *test;
kthread_sched_data_t *tsched = kthread_get_sched_param ( kthread );
ASSERT ( kthread );
test = kthreadq_remove ( &ksched_edf.params.edf.wait, kthread );
LOG( DEBUG, "%x %x [Deadline alarm]", kthread, test );
if( test == kthread )
{
if ( edf_check_deadline ( kthread ) )
{
LOG( DEBUG, "%x [Waked, but too late]", kthread );
kthread_set_syscall_retval ( kthread, -1 );
kthread_move_to_ready ( kthread, LAST );
if ( tsched->params.edf.flags & EDF_TERMINATE )
{
LOG( DEBUG, "%x [EDF_TERMINATE]", kthread );
tsched = kthread_get_sched_param ( kthread );
k_alarm_remove ( tsched->params.edf.edf_period_alarm );
k_alarm_remove ( tsched->params.edf.edf_deadline_alarm );
tsched->params.edf.edf_period_alarm = NULL;
kthread_cancel ( kthread, -E_DEADLINE );
}
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 );
if ( edf_check_deadline ( kthread ) )
{
/* what to do if its missed? kill thread? */
tsched = kthread_get_sched_param ( kthread );
if ( tsched->params.edf.flags & EDF_TERMINATE )
{
LOG( DEBUG, "%x [EDF_TERMINATE]", kthread );
k_alarm_remove ( tsched->params.edf.edf_period_alarm );
k_alarm_remove ( tsched->params.edf.edf_deadline_alarm );
tsched->params.edf.edf_period_alarm = NULL;
kthread_cancel ( kthread, -E_DEADLINE );
}
else if ( tsched->params.edf.flags & EDF_CONTINUE )
{
/* continue as deadline is not missed */
LOG( DEBUG, "%x [EDF_CONTINUE]", kthread );
}
else if ( tsched->params.edf.flags & EDF_SKIP )
{
/* skip deadline */
/* set times for next period */
LOG( DEBUG, "%x [EDF_SKIP]", kthread );
time_add ( &tsched->params.edf.next_run,
&tsched->params.edf.period );
tsched->params.edf.active_deadline = tsched->params.edf.next_run;
time_add ( &tsched->params.edf.active_deadline,
&tsched->params.edf.relative_deadline );
if ( kthread == ksched_edf.params.edf.active )
ksched_edf.params.edf.active = NULL;
alarm.action = edf_deadline_timer;
alarm.param = kthread;
alarm.flags = 0;
alarm.period.sec = alarm.period.nsec = 0;
alarm.exp_time = tsched->params.edf.active_deadline;
k_alarm_set ( tsched->params.edf.edf_deadline_alarm, &alarm );
alarm.action = edf_period_timer;
alarm.param = kthread;
alarm.flags = ALARM_PERIODIC;
alarm.period = tsched->params.edf.period;
alarm.exp_time = tsched->params.edf.next_run;
k_alarm_set ( tsched->params.edf.edf_period_alarm, &alarm );
kthread_enqueue ( kthread, &ksched_edf.params.edf.ready );
kthreads_schedule (); /* will call edf_schedule() */
}
}
}
}
static int edf_schedule ( ksched_t *ksched )
{
kthread_t *first, *next, *edf_active;
kthread_sched2_t *sch_first, *sch_next, *ea;
edf_active = ksched->params.edf.active;
if ( edf_active && !kthread_is_ready ( edf_active ) )
{
ksched->params.edf.active = edf_active = NULL;
}
first = kthreadq_get ( &ksched->params.edf.ready );
EDF_LOG ( "%x %x [active, first in queue]", edf_active, first );
if ( !first )
{
kthreads_schedule ();
return 0; /* no threads in edf.ready queue, edf.active unch. */
}
if ( edf_active )
{
next = first;
first = edf_active;
}
else {
next = kthreadq_get_next ( first );
}
while ( first && next )
{
sch_first = kthread_get_sched2_param ( first );
sch_next = kthread_get_sched2_param ( next );
if ( time_cmp ( &sch_first->params.edf.active_deadline,
&sch_next->params.edf.active_deadline ) > 0 )
{
first = next;
}
next = kthreadq_get_next ( next );
}
if ( first && first != edf_active )
{
next = kthreadq_remove ( &ksched->params.edf.ready, first );
EDF_LOG ( "%x removed, %x is now first", next,
kthreadq_get ( &ksched->params.edf.ready ) );
if ( edf_active )
{
EDF_LOG ( "%x=>%x [EDF_SCHED_PREEMPT]",
edf_active, first );
/*
* change active EDF thread:
* -remove it from active/ready list
* -put it into edf.ready list
*/
if ( kthread_is_ready (edf_active) )
{
if ( !kthread_is_active (edf_active) )
{
kthread_remove_from_ready (edf_active);
/*
* set "deactivated" flag, don't need
* another call to "edf_schedule"
*/
}
else {
ea = kthread_get_sched2_param (edf_active);
ea->activated = 0;
}
kthread_enqueue ( edf_active,
&ksched->params.edf.ready );
}
/* else = thread is blocked - leave it there */
}
ksched->params.edf.active = first;
EDF_LOG ( "%x [new active]", first );
kthread_move_to_ready ( first, LAST );
}
kthreads_schedule ();
return 0;
}
static int k_edf_schedule ()
{
kthread_t *first, *next, *edf_active;
kthread_sched_data_t *sch_first, *sch_next;
ksched_t *gsched = ksched_get ( SCHED_EDF );
int retval = 0;
edf_active = gsched->params.edf.active;
first = kthreadq_get ( &gsched->params.edf.ready );
LOG( DEBUG, "%x [active]", edf_active );
LOG( DEBUG, "%x [first]", first );
//LOG( DEBUG, "%x [next]", next );
if ( !first )
return 0; /* no threads in edf.ready queue, edf.active unch. */
if ( edf_active )
{
next = first;
first = edf_active;
LOG( DEBUG, "%x [next]", kthreadq_get_next ( next ) );
}
else {
next = kthreadq_get_next ( first );
LOG( DEBUG, "%x [next]", next );
}
while ( first && next )
{
sch_first = kthread_get_sched_param ( first );
sch_next = kthread_get_sched_param ( next );
if ( time_cmp ( &sch_first->params.edf.active_deadline,
&sch_next->params.edf.active_deadline ) > 0 )
{
first = next;
}
next = kthreadq_get_next ( next );
}
if ( first && first != edf_active )
{
next = kthreadq_remove ( &gsched->params.edf.ready, first );
LOG ( DEBUG, "%x removed, %x is now first", next, kthreadq_get ( &gsched->params.edf.ready ) );
if ( edf_active )
{
LOG( DEBUG, "%x=>%x [EDF_SCHED_PREEMPT]",
edf_active, first );
/*
* change active EDF thread:
* -remove it from active/ready list
* -put it into edf.ready list
*/
if ( kthread_is_ready (edf_active) )
{
if ( !kthread_is_active (edf_active) )
{
kthread_remove_from_ready (edf_active);
/*
* set "deactivated" flag, don't need
* another call to "edf_schedule"
*/
}
else {
kthread_get_sched_param (edf_active)
->activated = 0;
}
kthread_enqueue ( edf_active,
&gsched->params.edf.ready );
}
/* else = thread is blocked - leave it there */
}
gsched->params.edf.active = first;
LOG( DEBUG, "%x [new active]", first );
kthread_move_to_ready ( first, LAST );
retval = 1;
}
return retval;
}
static int edf_set_thread_sched_parameters (kthread_t *kthread, sched_t *params)
{
time_t now;
alarm_t alarm;
kthread_sched_data_t *tsched = kthread_get_sched_param ( kthread );
ksched_t *gsched = ksched_get ( SCHED_EDF );
if ( gsched->params.edf.active == kthread )
gsched->params.edf.active = NULL;
k_get_time ( &now );
if ( params->edf.flags & EDF_SET )
{
/*LOG( DEBUG, "%x [SET]", kthread ); */
tsched->params.edf.period = params->edf.period;
tsched->params.edf.relative_deadline = params->edf.deadline;
tsched->params.edf.flags = params->edf.flags;
/* set periodic alarm */
tsched->params.edf.next_run = now;
time_add ( &tsched->params.edf.next_run, ¶ms->edf.period );
edf_arm_deadline ( kthread );
edf_arm_period ( kthread );
/*
* adjust "next_run" and "deadline" for "0" period
* - first "edf_wait" will set correct values for first period
*/
tsched->params.edf.next_run = now;
time_sub ( &tsched->params.edf.next_run, ¶ms->edf.period );
tsched->params.edf.active_deadline = now;
time_add ( &tsched->params.edf.active_deadline,
¶ms->edf.deadline );
}
else if ( params->edf.flags & EDF_WAIT )
{
if ( edf_check_deadline ( kthread ) )
return -1;
/* set times for next period */
if ( time_cmp ( &now, &tsched->params.edf.next_run ) > 0 )
{
time_add ( &tsched->params.edf.next_run,
&tsched->params.edf.period );
tsched->params.edf.active_deadline = tsched->params.edf.next_run;
time_add ( &tsched->params.edf.active_deadline,
&tsched->params.edf.relative_deadline );
if ( kthread == gsched->params.edf.active )
gsched->params.edf.active = NULL;
/* set (separate) alarm for deadline */
alarm.action = edf_deadline_timer;
alarm.param = kthread;
alarm.flags = 0;
alarm.period.sec = alarm.period.nsec = 0;
alarm.exp_time = tsched->params.edf.active_deadline;
k_alarm_set ( tsched->params.edf.edf_deadline_alarm, &alarm );
}
/* is task ready for execution, or must wait until next period */
if ( time_cmp ( &tsched->params.edf.next_run, &now ) > 0 )
{
/* wait till "next_run" */
LOG( DEBUG, "%x [EDF WAIT]", kthread );
kthread_enqueue ( kthread, &gsched->params.edf.wait );
kthreads_schedule (); /* will call edf_schedule() */
}
else {
/* "next_run" has already come,
* activate task => move it to "EDF ready tasks"
*/
LOG( DEBUG, "%x [EDF READY]", kthread );
LOG( DEBUG, "%x [1st READY]", kthreadq_get ( &gsched->params.edf.ready ) );
kthread_enqueue ( kthread, &gsched->params.edf.ready );
kthreads_schedule (); /* will call edf_schedule() */
}
}
else if ( params->edf.flags & EDF_EXIT )
{
if ( kthread == gsched->params.edf.active )
gsched->params.edf.active = NULL;
//LOG( DEBUG, "%x [EXIT]", kthread );
if ( edf_check_deadline ( kthread ) )
{
LOG( DEBUG, "%x [EXIT-error]", kthread );
return -1;
}
LOG( DEBUG, "%x [EXIT-normal]", kthread );
if ( tsched->params.edf.edf_period_alarm )
k_alarm_remove ( tsched->params.edf.edf_period_alarm );
if ( tsched->params.edf.edf_deadline_alarm )
k_alarm_remove ( tsched->params.edf.edf_deadline_alarm );
tsched->sched_policy = SCHED_FIFO;
LOG( DEBUG, "%x [EXIT]", kthread );
if ( k_edf_schedule () )
{
LOG( DEBUG, "%x [EXIT]", kthread );
kthreads_schedule (); /* will NOT call edf_schedule() */
}
LOG( DEBUG, "%x [EXIT]", kthread );
}
return 0;
}
static void edf_deadline_alarm ( sigval_t sigev_value )
{
kthread_t *kthread = sigev_value.sival_ptr, *test;
kthread_sched2_t *tsched;
ksched_t *ksched;
itimerspec_t alarm;
ASSERT ( kthread );
ksched = ksched2_get ( kthread_get_sched_policy (kthread) );
tsched = kthread_get_sched2_param ( kthread );
test = kthreadq_remove ( &ksched->params.edf.wait, kthread );
EDF_LOG ( "%x %x [Deadline alarm]", kthread, test );
if( test == kthread )
{
EDF_LOG ( "%x [Waked, but too late]", kthread );
kthread_set_syscall_retval ( kthread, EXIT_FAILURE );
kthread_move_to_ready ( kthread, LAST );
if ( tsched->params.edf.flags & EDF_TERMINATE )
{
EDF_LOG ( "%x [EDF_TERMINATE]", kthread );
ktimer_delete ( tsched->params.edf.period_alarm );
tsched->params.edf.period_alarm = NULL;
ktimer_delete ( tsched->params.edf.deadline_alarm );
tsched->params.edf.deadline_alarm = NULL;
kthread_set_errno ( kthread, ETIMEDOUT );
kthread_exit ( kthread, NULL, TRUE );
}
else {
edf_schedule (ksched);
}
}
else {
/*
* thread is not in edf.wait queue, but might be running or its
* blocked - it is probable (almost certain) that it missed deadline
*/
EDF_LOG ( "%x [Not in edf.wait. Missed deadline?]", kthread );
if ( edf_check_deadline ( kthread ) )
{
/* what to do if its missed? kill thread? */
if ( tsched->params.edf.flags & EDF_TERMINATE )
{
EDF_LOG ( "%x [EDF_TERMINATE]", kthread );
ktimer_delete (tsched->params.edf.period_alarm);
tsched->params.edf.period_alarm = NULL;
ktimer_delete ( tsched->params.edf.deadline_alarm );
tsched->params.edf.deadline_alarm = NULL;
kthread_set_errno ( kthread, ETIMEDOUT );
kthread_exit ( kthread, NULL, TRUE );
}
else if ( tsched->params.edf.flags & EDF_CONTINUE )
{
/* continue as deadline is not missed */
EDF_LOG ( "%x [EDF_CONTINUE]", kthread );
}
else if ( tsched->params.edf.flags & EDF_SKIP )
{
/* skip deadline */
/* set times for next period */
EDF_LOG ( "%x [EDF_SKIP]", kthread );
time_add ( &tsched->params.edf.next_run,
&tsched->params.edf.period );
tsched->params.edf.active_deadline =
tsched->params.edf.next_run;
time_add ( &tsched->params.edf.active_deadline,
&tsched->params.edf.relative_deadline );
if ( kthread == ksched->params.edf.active )
ksched->params.edf.active = NULL;
TIME_RESET ( &alarm.it_interval );
alarm.it_value = tsched->params.edf.active_deadline;
ktimer_settime ( tsched->params.edf.deadline_alarm,
TIMER_ABSTIME, &alarm, NULL );
alarm.it_interval = tsched->params.edf.period;
alarm.it_value = tsched->params.edf.next_run;
ktimer_settime ( tsched->params.edf.period_alarm,
TIMER_ABSTIME, &alarm, NULL );
kthread_enqueue (kthread, &ksched->params.edf.ready);
edf_schedule (ksched);
}
} /* moved 1 tab left for readability */
}
}
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;
}
static int edf_set_thread_sched_parameters ( ksched_t *ksched,
kthread_t *kthread,
sched_supp_t *params )
{
timespec_t now;
itimerspec_t alarm;
kthread_sched2_t *tsched = kthread_get_sched2_param ( kthread );
if ( ksched->params.edf.active == kthread )
ksched->params.edf.active = NULL;
kclock_gettime ( CLOCK_REALTIME, &now );
if ( params->edf.flags & EDF_SET )
{
tsched->params.edf.period = params->edf.period;
tsched->params.edf.relative_deadline = params->edf.deadline;
tsched->params.edf.flags = params->edf.flags ^ EDF_SET;
/* create and set periodic alarm */
edf_create_alarm ( kthread, edf_period_alarm,
&tsched->params.edf.period_alarm );
tsched->params.edf.next_run = now;
time_add ( &tsched->params.edf.next_run, ¶ms->edf.period );
alarm.it_interval = tsched->params.edf.period;
alarm.it_value = tsched->params.edf.next_run;
ktimer_settime ( tsched->params.edf.period_alarm, TIMER_ABSTIME,
&alarm, NULL );
/* adjust "next_run" and "deadline" for "0" period
* first "edf_wait" will set correct values for first period */
tsched->params.edf.next_run = now;
time_sub ( &tsched->params.edf.next_run, ¶ms->edf.period );
/* create and set deadline alarm */
edf_create_alarm ( kthread, edf_deadline_alarm,
&tsched->params.edf.deadline_alarm );
tsched->params.edf.active_deadline = now;
time_add ( &tsched->params.edf.active_deadline,
¶ms->edf.deadline );
TIME_RESET ( &alarm.it_interval );
alarm.it_value = tsched->params.edf.active_deadline;
ktimer_settime ( tsched->params.edf.deadline_alarm,
TIMER_ABSTIME, &alarm, NULL );
/* move thread to edf scheduler */
kthread_enqueue ( kthread, &ksched->params.edf.ready );
edf_schedule (ksched);
}
else if ( params->edf.flags & EDF_WAIT )
{
if ( edf_check_deadline ( kthread ) )
return EXIT_FAILURE;
/* set times for next period */
if ( time_cmp ( &now, &tsched->params.edf.next_run ) > 0 )
{
time_add ( &tsched->params.edf.next_run,
&tsched->params.edf.period );
tsched->params.edf.active_deadline =
tsched->params.edf.next_run;
time_add ( &tsched->params.edf.active_deadline,
&tsched->params.edf.relative_deadline );
if ( kthread == ksched->params.edf.active )
ksched->params.edf.active = NULL;
/* set (separate) alarm for deadline
* (periodic alarm is set only once as periodic) */
TIME_RESET ( &alarm.it_interval );
alarm.it_value = tsched->params.edf.active_deadline;
ktimer_settime ( tsched->params.edf.deadline_alarm,
TIMER_ABSTIME, &alarm, NULL );
}
/* is task ready for execution, or must wait until next period*/
if ( time_cmp ( &tsched->params.edf.next_run, &now ) > 0 )
{
/* wait till "next_run" */
EDF_LOG ( "%x [EDF WAIT]", kthread );
kthread_enqueue ( kthread, &ksched->params.edf.wait );
edf_schedule (ksched);
}
else {
/* "next_run" has already come,
* activate task => move it to "EDF ready tasks"
*/
EDF_LOG ( "%x [EDF READY]", kthread );
kthread_enqueue ( kthread, &ksched->params.edf.ready );
edf_schedule (ksched);
}
}
else if ( params->edf.flags & EDF_EXIT )
{
if ( kthread == ksched->params.edf.active )
ksched->params.edf.active = NULL;
if ( edf_check_deadline ( kthread ) )
{
EDF_LOG ( "%x [EXIT-error]", kthread );
return EXIT_FAILURE;
}
EDF_LOG ( "%x [EXIT-normal]", kthread );
if ( tsched->params.edf.period_alarm )
{
/* disarm timer */
TIME_RESET ( &alarm.it_interval );
TIME_RESET ( &alarm.it_value );
ktimer_settime ( tsched->params.edf.period_alarm, 0,
&alarm, NULL );
}
if ( tsched->params.edf.deadline_alarm )
{
/* disarm timer */
TIME_RESET ( &alarm.it_interval );
TIME_RESET ( &alarm.it_value );
ktimer_settime ( tsched->params.edf.deadline_alarm, 0,
&alarm, NULL );
}
kthread_setschedparam ( kthread, SCHED_FIFO, NULL );
}
return 0;
}
