]> rtime.felk.cvut.cz Git - frescor/fosa.git/commitdiff
Allow for mutexes and condvars to be shared among different processes.
authorfaggioli <faggioli@35b4ef3e-fd22-0410-ab77-dab3279adceb>
Tue, 17 Feb 2009 13:00:13 +0000 (13:00 +0000)
committerfaggioli <faggioli@35b4ef3e-fd22-0410-ab77-dab3279adceb>
Tue, 17 Feb 2009 13:00:13 +0000 (13:00 +0000)
Bugfix in error handling in various functions.

git-svn-id: http://www.frescor.org/private/svn/frescor/fosa/trunk@1551 35b4ef3e-fd22-0410-ab77-dab3279adceb

src_aquosa/fosa_clocks_and_timers.c
src_aquosa/fosa_group_clocks.c
src_aquosa/fosa_long_jump.c
src_aquosa/fosa_mutexes_and_condvars.c
src_aquosa/fosa_threads_and_signals.c
src_aquosa/fosa_time.c

index 5adb0d6ba9fd8a29f25ed68bb01507e3f7d3e089..122d4e7102230acf3f9701a38cd17223f9883ef8 100644 (file)
@@ -88,9 +88,11 @@ int fosa_clock_get_time(fosa_clock_id_t clockid,
        struct timespec current_time_tspec;
 
        ret = clock_gettime(clockid, &current_time_tspec);
+       if (ret) return errno;
+
        *current_time = fosa_timespec_to_abs_time(current_time_tspec);
 
-       return ret;
+       return 0;
 }
 
 /**
@@ -107,13 +109,14 @@ int fosa_clock_get_time(fosa_clock_id_t clockid,
 int fosa_thread_get_cputime_clock(fosa_thread_id_t tid,
                                  fosa_clock_id_t *clockid)
 {
-       if (tid.linux_pid == tid.linux_tid) {
-               /* standard UNIX process */
-               return clock_getcpuclockid(tid.linux_pid, clockid);
-       } else {
-               /* POSIX thread */
-               return pthread_getcpuclockid(tid.pthread_id, clockid);
-       }
+       int ret;
+
+       if (tid.linux_pid == tid.linux_tid) /* standard UNIX process */
+               ret = clock_getcpuclockid(tid.linux_pid, clockid);
+       else /* POSIX thread */
+               ret = pthread_getcpuclockid(tid.pthread_id, clockid);
+
+       return ret ? errno : 0;
 }
 
 /*************************
@@ -153,13 +156,16 @@ int fosa_timer_create(fosa_clock_id_t clockid,
                      fosa_signal_info_t info,
                      fosa_timer_id_t *timerid)
 {
+       int ret;
        struct sigevent event;
 
        event.sigev_notify = SIGEV_SIGNAL;
        event.sigev_signo = signal;
        event.sigev_value = *((union sigval*) &info);
 
-       return timer_create(clockid, &event, timerid);
+       ret = timer_create(clockid, &event, timerid);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -179,6 +185,7 @@ int fosa_timer_create(fosa_clock_id_t clockid,
                                     fosa_timer_id_t *timerid,
                                     fosa_thread_id_t receiver)
 {
+       int ret;
        struct sigevent event;
 
        event.sigev_notify = SIGEV_SIGNAL;
@@ -187,7 +194,9 @@ int fosa_timer_create(fosa_clock_id_t clockid,
        event.sigev_notify = SIGEV_THREAD_ID | SIGEV_SIGNAL;
        event._sigev_un._tid = receiver.linux_tid;
 
-       return timer_create(clockid, &event, timerid);
+       ret = timer_create(clockid, &event, timerid);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -202,7 +211,11 @@ int fosa_timer_create(fosa_clock_id_t clockid,
  **/
 int fosa_timer_delete(fosa_timer_id_t  timerid)
 {
-       return timer_delete(timerid);
+       int ret;
+
+       ret = timer_delete(timerid);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -233,13 +246,16 @@ int fosa_timer_delete(fosa_timer_id_t  timerid)
  **/
 int fosa_rel_timer_arm(fosa_timer_id_t timerid, const fosa_rel_time_t *value)
 {
+       int ret;
        struct itimerspec when;
 
        /* non-periodic one shot timer */
        when.it_value = fosa_abs_time_to_timespec(*value);
        when.it_interval = zero_time;
 
-       return timer_settime(timerid, 0, &when, NULL);
+       ret = timer_settime(timerid, 0, &when, NULL);
+
+       return ret ? errno : 0;
 }
 
 
@@ -271,13 +287,16 @@ int fosa_rel_timer_arm(fosa_timer_id_t timerid, const fosa_rel_time_t *value)
  **/
 int fosa_abs_timer_arm(fosa_timer_id_t timerid, const fosa_abs_time_t *value)
 {
+       int ret;
        struct itimerspec when;
 
        /* non-periodic one shot timer */
        when.it_value = fosa_abs_time_to_timespec(*value);
        when.it_interval = zero_time;
 
-       return timer_settime(timerid, TIMER_ABSTIME, &when, NULL);
+       ret = timer_settime(timerid, TIMER_ABSTIME, &when, NULL);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -306,7 +325,7 @@ int fosa_timer_get_remaining_time(fosa_timer_id_t timerid,
        ret = timer_gettime(timerid, &time);
        *remaining_time = fosa_timespec_to_rel_time(time.it_value);
 
-       return ret;
+       return ret ? errno : 0;
 }
 
 /**
@@ -335,13 +354,15 @@ int fosa_timer_disarm(fosa_timer_id_t timerid,
                return FOSA_EINVAL;
 
        ret = timer_gettime(timerid, &time);
+       if (ret) return errno;
+
        *remaining_time = fosa_timespec_to_rel_time(time.it_value);
-       if (ret < 0)
-               return ret;
 
        time.it_value = zero_time;
        time.it_interval = zero_time;
-       
-       return timer_settime(timerid, 0, &time, NULL);
+
+       ret = timer_settime(timerid, 0, &time, NULL);
+
+       return ret ? errno : 0;
 }
 
index f0e8a38ed0c47e47e3e309038328c9f083734e7f..20c8832d61f53f25455a93da044176fcf5ed12d7 100644 (file)
@@ -87,7 +87,7 @@
  **/
 int fosa_thread_set_create(fosa_thread_set_id_t *set)
 {
-       return EINVAL;
+       return FOSA_EINVAL;
 }
     
 
@@ -111,7 +111,7 @@ int fosa_thread_set_create(fosa_thread_set_id_t *set)
  **/
 int fosa_thread_set_destroy(fosa_thread_set_id_t set)
 {
-       return EINVAL;
+       return FOSA_EINVAL;
 }
 
 
@@ -135,7 +135,7 @@ int fosa_thread_set_destroy(fosa_thread_set_id_t set)
  **/
 int fosa_thread_set_add(fosa_thread_set_id_t set, fosa_thread_id_t thread_id)
 {
-       return EINVAL;
+       return FOSA_EINVAL;
 }
 
 
@@ -159,7 +159,7 @@ int fosa_thread_set_add(fosa_thread_set_id_t set, fosa_thread_id_t thread_id)
 
 int fosa_thread_set_del(fosa_thread_set_id_t set, fosa_thread_id_t thread_id)
 {
-       return EINVAL;
+       return FOSA_EINVAL;
 }
 
 
@@ -184,6 +184,6 @@ int fosa_thread_set_del(fosa_thread_set_id_t set, fosa_thread_id_t thread_id)
 int fosa_get_groupcpu_clock(const fosa_thread_set_id_t set,
                            fosa_clock_id_t *clock_id)
 {
-       return EINVAL;
+       return FOSA_EINVAL;
 }
 
index b1e193a2af05006430b48b38c7600527fde61340..30e350e96d949fb166e67766ac7037d1f8826848 100644 (file)
@@ -132,7 +132,7 @@ int fosa_long_jump_save_context(fosa_long_jump_context_t *context)
 int fosa_long_jump_was_performed(const fosa_long_jump_context_t *context,
                                 int *jumped)
 {
-       return EINVAL;
+       return FOSA_EINVAL;
 }
 
 /** 
@@ -191,6 +191,7 @@ void __long_jump_handler(int n, siginfo_t *info, void *c)
 int fosa_long_jump_install_handler(fosa_signal_t *signal,
                                   fosa_thread_id_t *handler)
 {
+       int ret;
        struct sigaction sa_long_jump;
 
        sa_long_jump.sa_handler = NULL;
@@ -200,6 +201,8 @@ int fosa_long_jump_install_handler(fosa_signal_t *signal,
 
        handler = NULL;
 
-       return sigaction(*signal, &sa_long_jump, NULL);
+       ret = sigaction(*signal, &sa_long_jump, NULL);
+
+       return ret ? errno : 0;
 }
 
index 8b03c8d488cf8b41f1dd3a08fb7e266d43aa587e..889edc031cf000a21a10a87c131a2f0936f8883c 100644 (file)
@@ -59,6 +59,7 @@
 //================================================
 
 #include "fosa_time.h"
+#include "fosa_configuration_parameters.h"
 #include "fosa_mutexes_and_condvars.h"
 
 #ifdef OMK_FOR_USER            /* If compiled by OMK, use the config */
  **/
 int fosa_mutex_init(fosa_mutex_t *mutex, int prioceiling)
 {
-       int error;
+       int ret;
        pthread_mutexattr_t attr;
 
-       if ((error = pthread_mutexattr_init(&attr)) != 0)
-               return error;
+       ret = pthread_mutexattr_init(&attr);
+       if (ret) return errno;
 
 #ifndef CONFIG_NO_PRIO_INHERIT /* Valgrind doesn't support this attribute */
-       if ((error = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT)) != 0)
-               return error;
+       ret = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+       if (ret) return errno;
 #endif
+
+       ret = pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+       if (ret) return errno;
+
        return pthread_mutex_init(mutex, &attr);
 }
 
@@ -128,7 +133,7 @@ int fosa_mutex_set_prioceiling(fosa_mutex_t *mutex,
                               int new_ceiling,
                               int *old_ceiling)
 {
-       return -EINVAL;
+       return FOSA_EINVAL;
 }
 
 /**
@@ -142,7 +147,7 @@ int fosa_mutex_set_prioceiling(fosa_mutex_t *mutex,
  **/
 int fosa_mutex_get_prioceiling(const fosa_mutex_t *mutex, int *ceiling)
 {
-       return -EINVAL;
+       return FOSA_EINVAL;
 }
 
 /**
@@ -224,7 +229,19 @@ int fosa_mutex_unlock(fosa_mutex_t *mutex)
  **/
 int fosa_cond_init(fosa_cond_t *cond)
 {
-       return pthread_cond_init(cond, NULL);
+       int ret;
+       pthread_condattr_t attr;
+
+       ret = pthread_condattr_init(&attr);
+       if (ret) return errno;
+
+       ret = pthread_condattr_setclock(&attr, FOSA_CLOCK_REALTIME);
+       if (ret) return errno;
+
+       ret = pthread_condattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+       if (ret) return errno;
+
+       return pthread_cond_init(cond, &attr);
 }
 
 /**
@@ -321,6 +338,6 @@ int fosa_cond_timedwait(fosa_cond_t *cond,
        struct timespec abstime_tspec;
 
        abstime_tspec = fosa_abs_time_to_timespec(*abstime);
-
        return pthread_cond_timedwait(cond, mutex, &abstime_tspec);
 }
+
index 3f64a674dfd2c883ba99b58e0ec20e7b550bcbea..03e31160e9c69f305f4c32b842db9d9048554086 100644 (file)
@@ -73,20 +73,20 @@ static pthread_mutex_t key_lock;
 /* Initialize the keys data structure */
 int init_keys()
 {
-       int i, error;
+       int i, ret;
        pthread_mutexattr_t attr;
 
        for(i = 0; i < FOSA_MAX_KEYS; i++)
                key_in_use[i] = false;
 
-       if ((error = pthread_mutexattr_init(&attr)) != 0)
-               return error;
+       ret = pthread_mutexattr_init(&attr);
+       if (ret) return errno;
 
-       if ((error = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT)) != 0)
-               return error;
+       ret = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+       if (ret) return errno;
 
-       if ((error = pthread_mutex_init(&key_lock,&attr)) != 0)
-               return error;
+       ret = pthread_mutex_init(&key_lock,&attr);
+       if (ret) return errno;
 
        return 0;
 }
@@ -279,27 +279,30 @@ int fosa_thread_create(fosa_thread_id_t *tid,
  **/
 int fosa_key_create(int *key)
 {
-       int i, error = 0;
+       int i, ret;
        bool found = false;
 
-       if ((error = pthread_mutex_lock(&key_lock)) != 0)
-               return error;
+       ret = pthread_mutex_lock(&key_lock);
+       if (ret) return ret;
 
        /* find an unused key */
        for (i = 0; i < FOSA_MAX_KEYS; i++) {
                if (!key_in_use[i]) {
+                       ret = pthread_key_create(&(key_list[i]), NULL);
+                       if (ret) return ret;
+
                        *key = i;
                        key_in_use[i] = true;
-                       error = pthread_key_create(&(key_list[i]), NULL);
                        found = true;
+
                        break;
                }
        }
 
-       if ((error = pthread_mutex_unlock(&key_lock))!= 0)
-               return error;
+       ret = pthread_mutex_unlock(&key_lock);
+       if (ret) return ret;
 
-       return (!found ? FOSA_EINVAL : error);
+       return (!found ? FOSA_EINVAL : ret);
 }
 
 /**
@@ -314,16 +317,18 @@ int fosa_key_create(int *key)
  **/
 int fosa_key_destroy(int key)
 {
-       int error;
+       int ret;
 
-       if ((error = pthread_mutex_lock(&key_lock)) != 0)
-               return error;
+       ret = pthread_mutex_lock(&key_lock);
+       if (ret) return ret;
 
-       if ((error = pthread_key_delete(key_list[key])) != 0)
-               key_in_use[key]=false;
+       ret = pthread_key_delete(key_list[key]);
+       if (ret) return ret;
 
-       if ((error = pthread_mutex_unlock(&key_lock)) != 0)
-               return error;
+       key_in_use[key]=false;
+
+       ret = pthread_mutex_unlock(&key_lock);
+       if (ret) return ret;
 
        return 0;
 }
@@ -347,11 +352,15 @@ int fosa_key_destroy(int key)
                                   fosa_thread_id_t tid,
                                   const void * value)
 {
+       int ret;
+
        /* only POSIX threads can have specific data */
        if (!__fosa_check_thread(&tid))
-               return EINVAL;
+               return FOSA_EINVAL;
 
-       return pthread_setspecific(key_list[key], value);
+       ret = pthread_setspecific(key_list[key], value);
+
+       return ret ? ret : 0;
 }
 
 /**
@@ -376,10 +385,9 @@ int fosa_thread_get_specific_data(int key,
        if (!__fosa_check_thread(&tid))
                return EINVAL;
 
-       if ((value = pthread_getspecific(key_list[key])) != NULL)
-               return EINVAL;
-       else
-               return 0;
+       value = pthread_getspecific(key_list[key]);
+
+       return !value ? FOSA_EINVAL : 0;
 }
 
 /******************************************************************
@@ -400,7 +408,11 @@ int fosa_thread_get_specific_data(int key,
  **/
 int fosa_get_priority_max()
 {
-       return sched_get_priority_max(SCHED_RR);
+       int ret;
+
+       ret = sched_get_priority_max(SCHED_RR);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -410,7 +422,11 @@ int fosa_get_priority_max()
  **/
 int fosa_get_priority_min()
 {
-       return sched_get_priority_min(SCHED_RR);
+       int ret;
+
+       ret = sched_get_priority_min(SCHED_RR);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -431,12 +447,12 @@ int fosa_get_priority_min()
  **/
 int fosa_thread_attr_set_prio(fosa_thread_attr_t *attr, int prio)
 {
-       int error;
+       int ret;
        struct sched_param param;
 
        param.sched_priority = prio;
-       if ((error = pthread_attr_setschedpolicy(attr, SCHED_RR)) == 0)
-               return error;
+       ret = pthread_attr_setschedpolicy(attr, SCHED_RR);
+       if (ret) return ret;
 
        return pthread_attr_setschedparam(attr, &param);
 }
@@ -453,13 +469,15 @@ int fosa_thread_attr_set_prio(fosa_thread_attr_t *attr, int prio)
  **/
 int fosa_thread_attr_get_prio(const fosa_thread_attr_t *attr, int *prio)
 {
-       int error;
+       int ret;
        struct sched_param param;
 
-       if ((error = pthread_attr_getschedparam(attr, &param)) == 0)
-               *prio = param.sched_priority;
+       ret = pthread_attr_getschedparam(attr, &param);
+       if (ret) return ret;
+
+       *prio = param.sched_priority;
 
-       return error;
+       return 0;
 }
 
 /**
@@ -477,10 +495,14 @@ int fosa_thread_attr_get_prio(const fosa_thread_attr_t *attr, int *prio)
  **/
 int fosa_thread_set_prio(fosa_thread_id_t tid, int prio)
 {
+       int ret;
        struct sched_param param;
 
        param.sched_priority = prio;
-       return sched_setscheduler(0, SCHED_RR, &param);
+
+       ret = sched_setscheduler(0, SCHED_RR, &param);
+
+       return ret ? errno : 0;
 }
 
 /**
@@ -501,7 +523,7 @@ int fosa_thread_get_prio(fosa_thread_id_t tid, int *prio)
        ret = sched_getparam(0, &param);
        *prio = param.sched_priority;
 
-       return ret;
+       return ret ? errno : 0;
 }
 
 /*******************************************************************
@@ -537,13 +559,13 @@ int fosa_thread_get_prio(fosa_thread_id_t tid, int *prio)
  **/
 int fosa_set_accepted_signals(fosa_signal_t set[], int size)
 {
-       int i, error;
+       int i, ret;
        fosa_thread_id_t self;
        sigset_t sigset;
        struct sigaction action;
 
-       if ((error = sigemptyset(&sigset)) != 0)
-               return error;
+       ret = sigemptyset(&sigset);
+       if (ret) goto err;
 
        action.sa_handler = SIG_DFL;
        action.sa_mask = sigset;
@@ -551,17 +573,24 @@ int fosa_set_accepted_signals(fosa_signal_t set[], int size)
        action.sa_sigaction = NULL;
 
        for (i = 0; i < size; i++) {
-               if ((error = sigaddset(&sigset, set[i])) != 0)
-                       return error;
-               if ((error = sigaction(set[i], &action, NULL)) != 0)
-                       return error;
+               ret = sigaddset(&sigset, set[i]);
+               if (ret) goto err;
+               ret = sigaction(set[i], &action, NULL);
+               if (ret) goto err;
        }
 
        self = fosa_thread_self();
-       if (__fosa_check_thread(&self))
-               return pthread_sigmask(SIG_BLOCK, &sigset, NULL);
-       else
-               return sigprocmask(SIG_BLOCK, &sigset, NULL);
+       if (__fosa_check_thread(&self)) {
+               ret = pthread_sigmask(SIG_BLOCK, &sigset, NULL);
+               if (ret) return ret;
+       } else {
+               ret = sigprocmask(SIG_BLOCK, &sigset, NULL);
+               if (ret) goto err;
+       }
+
+       return 0;
+err:
+       return errno;
 }
 
 /**
@@ -601,12 +630,13 @@ int fosa_signal_queue(fosa_signal_t signal,
                      fosa_signal_info_t info,
                      fosa_thread_id_t receiver)
 {
-       int error;
+       int ret;
        timer_t fake_timer;
        struct itimerspec fake_time;
        struct sigevent fake_event;
 
-       timer_create(CLOCK_MONOTONIC, &fake_event, &fake_timer);
+       ret = timer_create(FOSA_CLOCK_REALTIME, &fake_event, &fake_timer);
+       if (ret) goto err;
 
         fake_time.it_value.tv_sec = fake_time.it_value.tv_nsec = 0;
         fake_time.it_interval.tv_sec = fake_time.it_interval.tv_nsec = 0;
@@ -615,10 +645,18 @@ int fosa_signal_queue(fosa_signal_t signal,
        fake_event.sigev_value.sival_int = info.sival_int;
         fake_event._sigev_un._tid = receiver.linux_tid;
 
-        error = timer_settime(fake_timer, TIMER_ABSTIME, &fake_time, NULL);
-       timer_delete(fake_timer);
+        ret = timer_settime(fake_timer, TIMER_ABSTIME, &fake_time, NULL);
+       if (ret) {
+               timer_delete(fake_timer);
+               goto err;
+       }
 
-       return error;
+       ret = timer_delete(fake_timer);
+       if (ret) goto err;
+
+       return 0;
+err:
+       return errno;
 }
 
 /**
@@ -649,19 +687,20 @@ int fosa_signal_wait(fosa_signal_t set[], int size,
                     fosa_signal_t *signal_received,
                     fosa_signal_info_t *info)
 {
-       int i, error;
+       int i, ret;
        sigset_t sigset;
        siginfo_t siginfo;
 
-       if ((error = sigemptyset(&sigset)) != 0)
-               return error;
+       ret = sigemptyset(&sigset);
+       if (ret) goto err;
 
-       for (i = 0; i < size; i++)
-               if ((error = sigaddset(&sigset,set[i])) != 0)
-                       return error;
+       for (i = 0; i < size; i++) {
+               ret = sigaddset(&sigset,set[i]);
+               if (ret) goto err;
+       }
 
-       if ((error = sigwaitinfo(&sigset, &siginfo)) != 0)
-               return error;
+       ret = sigwaitinfo(&sigset, &siginfo);
+       if (ret) goto err;
 
        if (info != NULL && signal_received != NULL)
                *signal_received = siginfo.si_signo;
@@ -669,6 +708,8 @@ int fosa_signal_wait(fosa_signal_t set[], int size,
                *info = (fosa_signal_info_t) siginfo.si_value.sival_int;
 
        return 0;
+err:
+       return errno;
 }
 
 /**
@@ -695,19 +736,20 @@ int fosa_signal_timedwait(fosa_signal_t set[], int size,
                          fosa_signal_info_t *info,
                          const struct timespec *timeout)
 {
-       int i, error;
+       int i, ret;
        sigset_t signalset;
        siginfo_t siginfo;
 
-       if ((error = sigemptyset(&signalset)) != 0)
-               return error;
+       ret = sigemptyset(&signalset);
+       if (ret) goto err;
 
-       for (i = 0; i < size; i++)
-               if ((error = sigaddset(&signalset,set[i])) != 0)
-                       return error;
+       for (i = 0; i < size; i++) {
+               ret = sigaddset(&signalset,set[i]);
+               if (ret) goto err;
+       }
 
-       if ((error = sigtimedwait(&signalset,&siginfo,timeout)) != 0)
-               return error;
+       ret = sigtimedwait(&signalset,&siginfo,timeout);
+       if (ret) goto err;
 
        if (signal_received != NULL)
                *signal_received = siginfo.si_signo;
@@ -715,5 +757,7 @@ int fosa_signal_timedwait(fosa_signal_t set[], int size,
                *info = (fosa_signal_info_t) siginfo.si_value.sival_int;
 
        return 0;
+err:
+       return errno;
 }
 
index 571ccce8ce787512ab4d7a4aecbb11f21b8e08a0..a259a375c8b1dfcd11f494ee8581994d6cf468c0 100644 (file)
  **/
 void inline fosa_eat(const struct timespec *cpu_time)
 {
-       int error;
+       int ret;
        fosa_thread_id_t self;
        clockid_t clock_id;
        struct timespec current_time, time_to_go;
 
        self = fosa_thread_self();
-       error = fosa_thread_get_cputime_clock(self, &clock_id);
-       if (!error)
-               return;
+       ret = fosa_thread_get_cputime_clock(self, &clock_id);
+       if (ret) return;
 
-       if (clock_gettime(clock_id, &current_time) != 0)
-                       return;
+       ret = clock_gettime(clock_id, &current_time);
+       if (ret) return;
 
        time_to_go = fosa_abs_time_incr(current_time, *cpu_time);
-       while (fosa_abs_time_smaller_or_equal(current_time, time_to_go))
-               if (clock_gettime(clock_id, &current_time) != 0)
-                       return;
+       while (fosa_abs_time_smaller_or_equal(current_time, time_to_go)) {
+               ret = clock_gettime(clock_id, &current_time);
+               if (ret) return;
+       }
 }