/*! 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; }