score: Simplify thread queue timeout handling

Add Thread_queue_Context::timeout_absolute to specify an absolute or
relative timeout.  This avoid having to get the current time twice for
timeouts relative to the current time.  It moves also functionality to
common code.

10 files changed, 88 insertions, 44 deletions

diff --git a/cpukit/include/rtems/score/threadq.h b/cpukit/include/rtems/score/threadq.h
index 5234019b81..10476888d4 100644
--- a/cpukit/include/rtems/score/threadq.h
+++ b/cpukit/include/rtems/score/threadq.h
@@ -214,7 +214,8 @@ struct Thread_queue_Context {
    * callout must be used to install the thread watchdog for timeout handling.
    *
    * @see _Thread_queue_Enqueue_do_nothing_extra().
-   *   _Thread_queue_Add_timeout_ticks(), and
+   *   _Thread_queue_Add_timeout_ticks(),
+   *   _Thread_queue_Add_timeout_monotonic_timespec(), and
    *   _Thread_queue_Add_timeout_realtime_timespec().
    */
   Thread_queue_Enqueue_callout enqueue_callout;
@@ -236,6 +237,12 @@ struct Thread_queue_Context {
     const void *arg;
   } Timeout;
 
+  /**
+   * @brief If this member is true, the timeout shall be absolute, otherwise it
+   *   shall be relative to the current time of the clock.
+   */
+  bool timeout_absolute;
+
 #if defined(RTEMS_SMP)
   /**
    * @brief Representation of a thread queue path from a start thread queue to
diff --git a/cpukit/include/rtems/score/threadqimpl.h b/cpukit/include/rtems/score/threadqimpl.h
index ca59de9e31..7b00de009d 100644
--- a/cpukit/include/rtems/score/threadqimpl.h
+++ b/cpukit/include/rtems/score/threadqimpl.h
@@ -267,41 +267,53 @@ _Thread_queue_Context_set_enqueue_timeout_ticks(
 }
 
 /**
- * @brief Sets the enqueue callout to add an absolute monotonic timeout in
- * timespec format.
+ * @brief Sets the enqueue callout to add a timeout in timespec format using
+ *   CLOCK_MONOTONIC.
  *
- * @param[out] queue_context The thread queue context.
- * @param abstime The absolute monotonic timeout.
+ * @param[out] queue_context is the thread queue context.
+ *
+ * @param timeout is the absolute or relative timeout.
+ *
+ * @param absolute is true, if the timeout shall be absolute, otherwise it
+ *   shall be relative to the current time of the clock.
  *
  * @see _Thread_queue_Enqueue().
  */
 RTEMS_INLINE_ROUTINE void
 _Thread_queue_Context_set_enqueue_timeout_monotonic_timespec(
   Thread_queue_Context  *queue_context,
-  const struct timespec *abstime
+  const struct timespec *timeout,
+  bool                   absolute
 )
 {
-  queue_context->Timeout.arg = abstime;
+  queue_context->Timeout.arg = timeout;
+  queue_context->timeout_absolute = absolute;
   queue_context->enqueue_callout =
     _Thread_queue_Add_timeout_monotonic_timespec;
 }
 
 /**
- * @brief Sets the enqueue callout to add an absolute realtime timeout in
- * timespec format.
+ * @brief Sets the enqueue callout to add a timeout in timespec format using
+ *   CLOCK_REALTIME.
  *
- * @param[out] queue_context The thread queue context.
- * @param abstime The absolute realtime timeout.
+ * @param[out] queue_context is the thread queue context.
+ *
+ * @param timeout is the absolute or relative timeout.
+ *
+ * @param absolute is true, if the timeout shall be absolute, otherwise it
+ *   shall be relative to the current time of the clock.
  *
  * @see _Thread_queue_Enqueue().
  */
 RTEMS_INLINE_ROUTINE void
 _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
   Thread_queue_Context  *queue_context,
-  const struct timespec *abstime
+  const struct timespec *timeout,
+  bool                   absolute
 )
 {
-  queue_context->Timeout.arg = abstime;
+  queue_context->Timeout.arg = timeout;
+  queue_context->timeout_absolute = absolute;
   queue_context->enqueue_callout = _Thread_queue_Add_timeout_realtime_timespec;
 }
 
diff --git a/cpukit/include/rtems/score/watchdogimpl.h b/cpukit/include/rtems/score/watchdogimpl.h
index a8e6de4fbe..7b364b8828 100644
--- a/cpukit/include/rtems/score/watchdogimpl.h
+++ b/cpukit/include/rtems/score/watchdogimpl.h
@@ -535,6 +535,22 @@ RTEMS_INLINE_ROUTINE uint64_t _Watchdog_Ticks_from_timespec(
 }
 
 /**
+ * @brief Converts the ticks to timespec.
+ *
+ * @param ticks are the ticks to convert.
+ *
+ * @param[out] ts is the timespec to return the converted ticks.
+ */
+RTEMS_INLINE_ROUTINE void _Watchdog_Ticks_to_timespec(
+  uint64_t         ticks,
+  struct timespec *ts
+)
+{
+  ts->tv_sec = ticks >> WATCHDOG_BITS_FOR_1E9_NANOSECONDS;
+  ts->tv_nsec = ticks & ( ( 1U << WATCHDOG_BITS_FOR_1E9_NANOSECONDS ) - 1 );
+}
+
+/**
  * @brief Converts the sbintime in ticks.
  *
  * @param sbt The sbintime to convert to ticks.
diff --git a/cpukit/posix/src/clocknanosleep.c b/cpukit/posix/src/clocknanosleep.c
index 588f67a60a..bfb78466df 100644
--- a/cpukit/posix/src/clocknanosleep.c
+++ b/cpukit/posix/src/clocknanosleep.c
@@ -43,11 +43,10 @@ int clock_nanosleep(
   struct timespec        *rmtp
 )
 {
-  Thread_queue_Context   queue_context;
-  struct timespec        now;
-  const struct timespec *end;
-  Thread_Control        *executing;
-  int                    eno;
+  Thread_queue_Context queue_context;
+  bool                 absolute;
+  Thread_Control      *executing;
+  int                  eno;
 
   if ( clock_id != CLOCK_REALTIME && clock_id != CLOCK_MONOTONIC ) {
     return ENOTSUP;
@@ -60,26 +59,23 @@ int clock_nanosleep(
   );
 
   if ( ( flags & TIMER_ABSTIME ) != 0 ) {
-    end = rqtp;
+    absolute = true;
+    rmtp = NULL;
   } else {
-    if ( clock_id == CLOCK_REALTIME ) {
-      _Timecounter_Nanotime( &now );
-    } else {
-      _Timecounter_Nanouptime( &now );
-    }
-
-    end = _Watchdog_Future_timespec( &now, rqtp );
+    absolute = false;
   }
 
   if ( clock_id == CLOCK_REALTIME ) {
     _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
       &queue_context,
-      end
+      rqtp,
+      absolute
     );
   } else {
     _Thread_queue_Context_set_enqueue_timeout_monotonic_timespec(
       &queue_context,
-      end
+      rqtp,
+      absolute
     );
   }
 
@@ -97,10 +93,11 @@ int clock_nanosleep(
     eno = 0;
   }
 
-  if ( rmtp != NULL && ( flags & TIMER_ABSTIME ) == 0 ) {
+  if ( rmtp != NULL ) {
 #if defined( RTEMS_POSIX_API )
     if ( eno == EINTR ) {
       struct timespec actual_end;
+      struct timespec planned_end;
 
       if ( clock_id == CLOCK_REALTIME ) {
         _Timecounter_Nanotime( &actual_end );
@@ -108,8 +105,13 @@ int clock_nanosleep(
         _Timecounter_Nanouptime( &actual_end );
       }
 
-      if ( _Timespec_Less_than( &actual_end, end ) ) {
-        _Timespec_Subtract( &actual_end, end, rmtp );
+      _Watchdog_Ticks_to_timespec(
+        executing->Timer.Watchdog.expire,
+        &planned_end
+      );
+
+      if ( _Timespec_Less_than( &actual_end, &planned_end ) ) {
+        _Timespec_Subtract( &actual_end, &planned_end, rmtp );
       } else {
         _Timespec_Set_to_zero( rmtp );
       }
diff --git a/cpukit/posix/src/prwlocktimedrdlock.c b/cpukit/posix/src/prwlocktimedrdlock.c
index 79059800bf..809f355359 100644
--- a/cpukit/posix/src/prwlocktimedrdlock.c
+++ b/cpukit/posix/src/prwlocktimedrdlock.c
@@ -37,7 +37,8 @@ int pthread_rwlock_timedrdlock(
   _Thread_queue_Context_initialize( &queue_context );
   _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
     &queue_context,
-    abstime
+    abstime,
+    true
   );
   status = _CORE_RWLock_Seize_for_reading(
     &the_rwlock->RWLock,
diff --git a/cpukit/posix/src/prwlocktimedwrlock.c b/cpukit/posix/src/prwlocktimedwrlock.c
index 9fb9a880a0..614d230ba9 100644
--- a/cpukit/posix/src/prwlocktimedwrlock.c
+++ b/cpukit/posix/src/prwlocktimedwrlock.c
@@ -39,7 +39,8 @@ int pthread_rwlock_timedwrlock(
   _Thread_queue_Context_initialize( &queue_context );
   _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
     &queue_context,
-    abstime
+    abstime,
+    true
   );
   status = _CORE_RWLock_Seize_for_writing(
     &the_rwlock->RWLock,
diff --git a/cpukit/posix/src/semtimedwait.c b/cpukit/posix/src/semtimedwait.c
index 21a8320b50..ae83e90540 100644
--- a/cpukit/posix/src/semtimedwait.c
+++ b/cpukit/posix/src/semtimedwait.c
@@ -60,7 +60,8 @@ int sem_timedwait(
     );
     _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
       &queue_context,
-      abstime
+      abstime,
+      true
     );
     _Thread_queue_Context_set_ISR_level( &queue_context, level );
     _Thread_queue_Enqueue(
diff --git a/cpukit/posix/src/sigtimedwait.c b/cpukit/posix/src/sigtimedwait.c
index 4e2b6c2658..0bdb65fd45 100644
--- a/cpukit/posix/src/sigtimedwait.c
+++ b/cpukit/posix/src/sigtimedwait.c
@@ -76,7 +76,6 @@ int sigtimedwait(
   siginfo_t             signal_information;
   siginfo_t            *the_info;
   int                   signo;
-  struct timespec       uptime;
   Thread_queue_Context  queue_context;
   int                   error;
 
@@ -93,13 +92,10 @@ int sigtimedwait(
    */
 
   if ( timeout != NULL ) {
-    const struct timespec *end;
-
-    _Timecounter_Nanouptime( &uptime );
-    end = _Watchdog_Future_timespec( &uptime, timeout );
     _Thread_queue_Context_set_enqueue_timeout_monotonic_timespec(
       &queue_context,
-      end
+      timeout,
+      false
     );
   } else {
     _Thread_queue_Context_set_enqueue_do_nothing_extra( &queue_context );
diff --git a/cpukit/score/src/mutex.c b/cpukit/score/src/mutex.c
index 88a390f323..f7e35093b2 100644
--- a/cpukit/score/src/mutex.c
+++ b/cpukit/score/src/mutex.c
@@ -206,7 +206,8 @@ int _Mutex_Acquire_timed(
   } else {
     _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
       &queue_context,
-      abstime
+      abstime,
+      true
     );
     _Mutex_Acquire_slow( mutex, owner, executing, level, &queue_context );
 
@@ -327,7 +328,8 @@ int _Mutex_recursive_Acquire_timed(
   } else {
     _Thread_queue_Context_set_enqueue_timeout_realtime_timespec(
       &queue_context,
-      abstime
+      abstime,
+      true
     );
     _Mutex_Acquire_slow( &mutex->Mutex, owner, executing, level, &queue_context );
 
diff --git a/cpukit/score/src/threadqtimeout.c b/cpukit/score/src/threadqtimeout.c
index ec8f67c93b..a3aeea43be 100644
--- a/cpukit/score/src/threadqtimeout.c
+++ b/cpukit/score/src/threadqtimeout.c
@@ -10,7 +10,7 @@
  */
 
 /*
- * Copyright (c) 2016, 2017 embedded brains GmbH
+ * Copyright (c) 2016, 2021 embedded brains GmbH
  *
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
@@ -55,8 +55,14 @@ static void _Thread_queue_Add_timeout_timespec(
 )
 {
   const struct timespec *abstime;
+  struct timespec        base;
 
-  abstime = queue_context->Timeout.arg;
+  if ( queue_context->timeout_absolute ) {
+    abstime = queue_context->Timeout.arg;
+  } else {
+    base = *now;
+    abstime = _Watchdog_Future_timespec( &base, queue_context->Timeout.arg );
+  }
 
   if ( _Watchdog_Is_valid_timespec( abstime ) ) {
     uint64_t expire;