/* Add alarm to alarm pool if its expiration time is defined
If expiration time is in the past, alarm will be immediately activated!
*/
static void k_alarm_add ( kalarm_t *kalarm )
{
int reschedule = 0;
/* if exp_time is given (>0) add it into active alarms */
if ( kalarm->alarm.exp_time.sec + kalarm->alarm.exp_time.nsec > 0 )
{
kalarm->active = 1;
list_sort_add ( &kalarms, kalarm, &kalarm->list, alarm_cmp );
}
else {
kalarm->active = 0;
}
reschedule = k_schedule_alarms (); /* this or other alarm may expire */
SET_ERRNO ( SUCCESS );
/* block thread? */
if ( kalarm->active && ( kalarm->alarm.flags & IPC_WAIT ) )
{
k_enqueue_thread ( NULL, &kalarm->queue );
reschedule = 1;
}
if ( reschedule )
k_schedule_threads ();
}
/*! Iterate through active alarms and activate newly expired ones */
static void k_schedule_alarms ()
{
kalarm_t *first;
time_t time, ref_time;
arch_get_time ( &time );
ref_time = time;
time_add ( &ref_time, &threshold );
/* should any alarm be activated? */
first = list_get ( &kalarms, FIRST );
while ( first != NULL )
{
if ( time_cmp ( &first->alarm.exp_time, &ref_time ) <= 0 )
{
/* 'activate' alarm */
/* but first remove alarm from list */
first = list_remove ( &kalarms, FIRST, NULL );
if ( first->alarm.flags & ALARM_PERIODIC )
{
/* calculate next activation time */
time_add ( &first->alarm.exp_time,
&first->alarm.period );
/* put back into list */
list_sort_add ( &kalarms, first, &first->list,
alarm_cmp );
}
else {
first->active = 0;
}
if ( first->alarm.action ) /* activate alarm */
first->alarm.action ( first->alarm.param );
first = list_get ( &kalarms, FIRST );
}
else {
break;
}
}
first = list_get ( &kalarms, FIRST );
if ( first )
{
ref_time = first->alarm.exp_time;
time_sub ( &ref_time, &time );
arch_timer_set ( &ref_time, k_timer_interrupt );
}
}
/*!
* Add alarm to alarm pool if its expiration time is defined
* If expiration time is in the past, alarm will be immediately activated!
*/
static void k_alarm_add ( kalarm_t *kalarm )
{
/* if exp_time is given (>0) add it into active alarms */
if ( kalarm->alarm.exp_time.sec + kalarm->alarm.exp_time.nsec > 0 )
{
kalarm->active = 1;
list_sort_add ( &kalarms, kalarm, &kalarm->list, alarm_cmp );
}
else {
kalarm->active = 0;
}
k_schedule_alarms (); /* this or other alarm may expire */
}
/*!
* Arm/disarm timer
* \param ktimer Timer
* \param flags Various flags
* \param value Set timer values (it_value+it_period)
* \param ovalue Where to store time to next timer expiration (+period)
* \return status 0 for success
*/
int ktimer_settime ( ktimer_t *ktimer, int flags, itimerspec_t *value,
itimerspec_t *ovalue )
{
timespec_t now;
ASSERT ( ktimer );
kclock_gettime ( ktimer->clockid, &now );
if ( ovalue )
{
*ovalue = ktimer->itimer;
/* return relative time to timer expiration */
if ( TIME_IS_SET ( &ovalue->it_value ) )
time_sub ( &ovalue->it_value, &now );
}
/* first disarm timer, if it was armed */
if ( TIMER_IS_ARMED ( ktimer ) )
{
TIMER_DISARM ( ktimer );
list_remove ( &ktimers, 0, &ktimer->list );
}
if ( value && TIME_IS_SET ( &value->it_value ) )
{
/* arm timer */
ktimer->itimer = *value;
if ( !(flags & TIMER_ABSTIME) ) /* convert to absolute time */
time_add ( &ktimer->itimer.it_value, &now );
list_sort_add ( &ktimers, ktimer, &ktimer->list, ktimer_cmp );
}
ktimer_schedule ();
return EXIT_SUCCESS;
}
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;
}
/*! 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 ();
}
/*!
* Iterate through active alarms and activate newly expired ones
*/
static int k_schedule_alarms ()
{
kalarm_t *first;
time_t time, ref_time;
int resched_thr = 0;
kprocess_t *proc;
arch_get_time ( &time );
ref_time = time;
time_add ( &ref_time, &threshold );
/* should any alarm be activated? */
first = list_get ( &kalarms, FIRST );
while ( first != NULL )
{
if ( time_cmp ( &first->alarm.exp_time, &ref_time ) <= 0 )
{
/* 'activate' alarm */
/* but first remove alarm from list */
first = list_remove ( &kalarms, FIRST, NULL );
if ( first->alarm.flags & ALARM_PERIODIC )
{
/* calculate next activation time */
time_add ( &first->alarm.exp_time,
&first->alarm.period );
/* put back into list */
list_sort_add ( &kalarms, first, &first->list,
alarm_cmp );
}
else {
first->active = 0;
}
if ( first->alarm.action )
{
/* call directly:
first->alarm.action ( first->alarm.param );
or create new thread for that job: */
if ( first->thread )
{ /* alarm scheduled by thread */
proc = k_get_thread_process ( first->thread );
k_create_thread (
first->alarm.action,
first->alarm.param,
proc->pi->exit,
k_get_thread_prio ( first->thread ) + 1,
NULL, 0, 1,
proc
);
resched_thr++;
}
else { /* alarm scheduled by kernel */
first->alarm.action ( first->alarm.param );
}
}
resched_thr += k_release_all_threads ( &first->queue );
first = list_get ( &kalarms, FIRST );
}
else {
break;
}
}
first = list_get ( &kalarms, FIRST );
if ( first )
{
ref_time = first->alarm.exp_time;
time_sub ( &ref_time, &time );
arch_timer_set ( &ref_time, k_timer_interrupt );
}
return resched_thr;
}
