diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c index a51ae70..14ec8b8 100644 --- a/rcl/src/rcl/allocator.c +++ b/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 * diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 8db5d26..debdaf4 100644 --- a/rcl/src/rcl/time_win32.c +++ b/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: - * https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 - * - * > Win32 implementation - * unless I've missed something obvious, the only way to get high-precision - * time on Windows is via the QueryPerformanceCounter() call. However, - * this is somewhat problematic in Windows XP on some processors, especially - * AMD, because the Windows implementation can freak out when the CPU clocks - * down to save power. Time can jump or even go backwards. Microsoft has - * fixed this bug for most systems now, but it can still show up if you have - * 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. - } + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + ULARGE_INTEGER uli; + uli.LowPart = ft.dwLowDateTime; + uli.HighPart = ft.dwHighDateTime; + // Adjust for January 1st, 1970, see: + // https://support.microsoft.com/en-us/kb/167296 + uli.QuadPart -= 116444736000000000; + // Convert to nanoseconds from 100's of nanoseconds. + now->nanoseconds = uli.QuadPart * 100; 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; }