implement time_win32.c

rcl/src/rcl/allocator.c

@@ -27,7 +27,7 @@ static void
 __default_deallocate(void * pointer, void * state)
 {
   (void)state;  // unused
-  return free(pointer);
+  free(pointer);
 }
 
 static void *

rcl/src/rcl/time_win32.c

@@ -30,101 +30,38 @@ extern "C"
 #include "./stdatomic_helper.h"
 #include "rcl/error_handling.h"
 
-#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0))
-
 rcl_ret_t
 rcl_system_time_point_now(rcl_system_time_point_t * now)
-{
-
-}
-#if 0
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
-  /* Windows implementation adapted from roscpp_core (3-clause BSD), see:
+  FILETIME ft;
-   *   https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96
+  GetSystemTimeAsFileTime(&ft);
-   *
+  ULARGE_INTEGER uli;
-   * > Win32 implementation
+  uli.LowPart = ft.dwLowDateTime;
-   *   unless I've missed something obvious, the only way to get high-precision
+  uli.HighPart = ft.dwHighDateTime;
-   *   time on Windows is via the QueryPerformanceCounter() call. However,
+  // Adjust for January 1st, 1970, see:
-   *   this is somewhat problematic in Windows XP on some processors, especially
+  //   https://support.microsoft.com/en-us/kb/167296
-   *   AMD, because the Windows implementation can freak out when the CPU clocks
+  uli.QuadPart -= 116444736000000000;
-   *   down to save power. Time can jump or even go backwards. Microsoft has
+  // Convert to nanoseconds from 100's of nanoseconds.
-   *   fixed this bug for most systems now, but it can still show up if you have
+  now->nanoseconds = uli.QuadPart * 100;
-   *   not installed the latest CPU drivers (an oxymoron). They fixed all these
-   *   problems in Windows Vista, and this API is by far the most accurate that
-   *   I know of in Windows, so I'll use it here despite all these caveats
-   *
-   * I've further modified it to be thread safe using a atomic_uint_least64_t.
-   */
-  LARGE_INTEGER cpu_freq;
-  static LARGE_INTEGER cpu_freq_global;
-  LARGE_INTEGER init_cpu_time;
-  static LARGE_INTEGER init_cpu_time_global;
-  static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0);
-  rcl_time_t start = {0, 0};
-  // If start_ns (static/global) is 0, then set it up on the first call.
-  uint64_t start_ns_loaded = rcl_atomic_load(&start_ns);
-  if (start_ns_loaded == 0) {
-    QueryPerformanceFrequency(&cpu_freq);
-    if (cpu_freq.QuadPart == 0) {
-      RCL_SET_ERROR_MSG("no high performance timer found");
-      return RCL_RET_ERROR;
-    }
-    QueryPerformanceCounter(&init_cpu_time);
-    // compute an offset from the Epoch using the lower-performance timer API
-    FILETIME ft;
-    GetSystemTimeAsFileTime(&ft);
-    LARGE_INTEGER start_li;
-    start_li.LowPart = ft.dwLowDateTime;
-    start_li.HighPart = ft.dwHighDateTime;
-    // why did they choose 1601 as the time zero, instead of 1970?
-    // there were no outstanding hard rock bands in 1601.
-#ifdef _MSC_VER
-    start_li.QuadPart -= 116444736000000000Ui64;
-#else
-    start_li.QuadPart -= 116444736000000000ULL;
-#endif
-    start.sec = (uint64_t)(start_li.QuadPart / 10000000);  // 100-ns units. odd.
-    start.nsec = (start_li.LowPart % 10000000) * 100;
-    static uint64_t expected = 0;
-    uint64_t desired = RCL_S_TO_NS(start.sec) + start.nsec;
-    if (rcl_atomic_compare_exchange_strong_uint_least64_t(&start_ns, &expected, desired)) {
-      // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals.
-      init_cpu_time_global = init_cpu_time;
-      cpu_freq_global = cpu_freq;
-    } else {
-      // Another concurrent first call managed to set this up first; reset start so it gets set.
-      start = {0, 0};
-    }
-  }
-  if (start.sec == 0 && start.nsec == 0) {
-    start.sec = RCL_NS_TO_S(start_ns_loaded);
-    start.nsec = start_ns_loaded % 1000000000;
-  }
-  LARGE_INTEGER cur_time;
-  QueryPerformanceCounter(&cur_time);
-  LARGE_INTEGER delta_cpu_time;
-  delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart;
-  double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart;
-  uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time);
-  uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9);
-
-  *now = start;
-  now->sec += delta_sec;
-  now->nsec += delta_nsec;
-
-  if (ret != RCL_RET_OK) {
-    return ret;  // rcl error state should already be set.
-  }
   return RCL_RET_OK;
 }
-#endif
 
 rcl_ret_t
 rcl_steady_time_point_now(rcl_steady_time_point_t * now)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
-  // WINAPI ret = QueryPerformanceFrequency();
+  LARGE_INTEGER cpu_frequency, performance_count;
+  // These should not ever fail since XP is already end of life:
+  // From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and
+  //      https://msdn.microsoft.com/en-us/library/windows/desktop/ms644904(v=vs.85).aspx:
+  // "On systems that run Windows XP or later, the function will always succeed and will
+  //  thus never return zero."
+  QueryPerformanceFrequency(&cpu_frequency);
+  QueryPerformanceCounter(&performance_count);
+  // Convert to nanoseconds before converting from ticks to avoid precision loss.
+  now->nanoseconds = RCL_S_TO_NS(performance_count.QuadPart);
+  now->nanoseconds /= cpu_frequency.QuadPart;
   return RCL_RET_OK;
 }