[PATCH] posix: fix clock_nanosleep and nanosleep clock use

[Gedare Bloom]

Sleeping with CLOCK_REALTIME should use the WATCHDOG_ABSOLUTE
clock discipline for the threadq so that the timeout interval
may change in case the clock source changes. Similarly,
CLOCK_MONOTONIC uses the WATCHDOG_RELATIVE threadq that will
only wakeup the thread after the requested count of ticks elapse.

updates #2732
---
 cpukit/posix/src/nanosleep.c | 67 +++++++++++++++++++++++++++++++++-----------
 1 file changed, 51 insertions(+), 16 deletions(-)

diff --git a/cpukit/posix/src/nanosleep.c b/cpukit/posix/src/nanosleep.c
index e60dd53..ce0100f 100644
--- a/cpukit/posix/src/nanosleep.c
+++ b/cpukit/posix/src/nanosleep.c
@@ -33,9 +33,9 @@ static Thread_queue_Control _Nanosleep_Pseudo_queue =
   THREAD_QUEUE_INITIALIZER( "Nanosleep" );
 
 static inline int nanosleep_helper(
-  const struct timespec  *rqtp,
-  struct timespec        *rmtp,
-  Watchdog_Discipline     discipline
+  uint64_t              ticks,
+  struct timespec      *rmtp,
+  Watchdog_Discipline   discipline
 )
 {
   /*
@@ -45,11 +45,9 @@ static inline int nanosleep_helper(
   Thread_Control  *executing;
   Per_CPU_Control *cpu_self;
 
-  Watchdog_Interval  ticks;
   Watchdog_Interval  start;
   Watchdog_Interval  elapsed;
 
-
   /*
    *  Return EINVAL if the delay interval is negative.
    *
@@ -59,11 +57,6 @@ static inline int nanosleep_helper(
   if ( !_Timespec_Is_valid( rqtp ) )
     return EINVAL;
 
-  /*
-   * Convert the timespec delay into the appropriate number of clock ticks.
-   */
-  ticks = _Timespec_To_ticks( rqtp );
-
   executing = _Thread_Get_executing();
 
   /*
@@ -110,7 +103,7 @@ static inline int nanosleep_helper(
   /*
    * If the user wants the time remaining, do the conversion.
    */
-  if ( rmtp && discipline == WATCHDOG_RELATIVE ) {
+  if ( rmtp ) {
     _Timespec_From_ticks( ticks, rmtp );
   }
 
@@ -135,8 +128,22 @@ int nanosleep(
   struct timespec        *rmtp
 )
 {
-  int err = nanosleep_helper(rqtp, rmtp, WATCHDOG_RELATIVE);
-  if (err) {
+  int err;
+  struct timespec timeout;
+  uint64_t ticks;
+
+ /* CLOCK_REALTIME can be adjusted during the timeout,
+  * so convert to an absolute timeout value and put the
+  * thread on the WATCHDOG_ABSOLUTE threadq. */
+  err = clock_gettime( CLOCK_REALTIME, &timeout );
+  if ( err != 0 )
+    return -1;
+  }
+
+  _Timespec_Add_to( &timeout, rqtp );
+  ticks = _Watchdog_Ticks_from_timespec( &timeout );
+  err = nanosleep_helper(ticks, rmtp, WATCHDOG_ABSOLUTE );
+  if ( err != 0 ) {
     rtems_set_errno_and_return_minus_one( err );
   }
   return 0;
@@ -153,12 +160,40 @@ int clock_nanosleep(
 )
 {
   int err = 0;
-  if ( clock_id == CLOCK_REALTIME || clock_id == CLOCK_MONOTONIC ) {
+  struct timespec absolute_timeout;
+  uint64_t ticks;
+  Watchdog_Interval relative_ticks;
+  TOD_Absolute_timeout_conversion_results status;
+
+  if ( flags & TIMER_ABSTIME ) {
+    /* See if absolute time already passed */
+    status = _TOD_Absolute_timeout_to_ticks(rqtp, clock_id, &relative_ticks);
+    if ( status == TOD_ABSOLUTE_TIMEOUT_INVALID )
+      return EINVAL;
+    if ( status == TOD_ABSOLUTE_TIMEOUT_IS_IN_PAST ||
+        status == TOD_ABSOLUTE_TIMEOUT_IS_NOW ) {
+      return 0;
+    }
+    rmtp = NULL; /* Do not touch rmtp when using absolute time */
+  } else {
+    relative_ticks = _Timespec_To_ticks(rqtp);
+  }
+
+  if ( clock_id == CLOCK_REALTIME ) {
     if ( flags & TIMER_ABSTIME ) {
-      err = nanosleep_helper(rqtp, rmtp, WATCHDOG_ABSOLUTE);
+      ticks = _Watchdog_Ticks_from_timespec(rqtp);
     } else {
-      err = nanosleep_helper(rqtp, rmtp, WATCHDOG_RELATIVE);
+      err = clock_gettime( CLOCK_REALTIME, &absolute_timeout );
+      if ( err != 0 ) {
+        return EINVAL;
+      }
+      _Timespec_Add_to( &absolute_timeout, rqtp );
+      ticks = _Watchdog_Ticks_from_timespec( &absolute_timeout );
     }
+    err = nanosleep_helper( ticks, rmtp, WATCHDOG_ABSOLUTE );
+  } else if ( clock_id == CLOCK_MONOTONIC ) {
+    /* use the WATCHDOG_RELATIVE to ignore changes in wall time */
+    err = nanosleep_helper( relative_ticks, rmtp, WATCHDOG_RELATIVE );
   } else {
     err = ENOTSUP;
   }
-- 
1.9.1