niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

wip

Browse source
This commit is contained in:
William Woodall 2015-12-11 18:44:46 -08:00
parent 879f0a1ced
commit d281ab3d28
6 changed files with 259 additions and 179 deletions
Download patch file Download diff file Expand all files Collapse all files

17
rcl/src/rcl/rcl.c
View file

@ -42,14 +42,15 @@ __clean_up_init()
} }
__rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state); __rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state);
} }
atomic_store(&__rcl_instance_id, 0); rcl_atomic_store(&__rcl_instance_id, 0);
atomic_store(&__rcl_is_initialized, false); rcl_atomic_store(&__rcl_is_initialized, false);
} }
rcl_ret_t rcl_ret_t
rcl_init(int argc, char ** argv, rcl_allocator_t allocator) rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
{ {
bool was_initialized = atomic_exchange(&__rcl_is_initialized, true); bool was_initialized;
rcl_atomic_exchange(&__rcl_is_initialized, was_initialized, true);
if (was_initialized) { if (was_initialized) {
RCL_SET_ERROR_MSG("rcl_init called while already initialized"); RCL_SET_ERROR_MSG("rcl_init called while already initialized");
return RCL_RET_ALREADY_INIT; return RCL_RET_ALREADY_INIT;
@ -67,8 +68,8 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
__rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state);
strcpy(__rcl_argv[i], argv[i]); // NOLINT(runtime/printf) strcpy(__rcl_argv[i], argv[i]); // NOLINT(runtime/printf)
} }
atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); rcl_atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id);
if (atomic_load(&__rcl_instance_id) == 0) { if (rcl_atomic_load_uint64_t(&__rcl_instance_id) == 0) {
// Roll over occurred. // Roll over occurred.
__rcl_next_unique_id--; // roll back to avoid the next call succeeding. __rcl_next_unique_id--; // roll back to avoid the next call succeeding.
RCL_SET_ERROR_MSG("unique rcl instance ids exhausted"); RCL_SET_ERROR_MSG("unique rcl instance ids exhausted");
@ -83,7 +84,7 @@ fail:
rcl_ret_t rcl_ret_t
rcl_shutdown() rcl_shutdown()
{ {
if (!atomic_load(&__rcl_is_initialized)) { if (!rcl_ok()) {
RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init"); RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init");
return RCL_RET_NOT_INIT; return RCL_RET_NOT_INIT;
} }
@ -94,13 +95,13 @@ rcl_shutdown()
uint64_t uint64_t
rcl_get_instance_id() rcl_get_instance_id()
{ {
return atomic_load(&__rcl_instance_id); return rcl_atomic_load_uint64_t(&__rcl_instance_id);
} }
bool bool
rcl_ok() rcl_ok()
{ {
return atomic_load(&__rcl_is_initialized); return rcl_atomic_load_bool(&__rcl_is_initialized);
} }
#if __cplusplus #if __cplusplus

66
rcl/src/rcl/stdatomic_helper.h
View file

@ -12,15 +12,73 @@
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#ifndef RCL__STDATOMICS_HELPER_H_ #ifndef RCL__STDATOMIC_HELPER_H_
#define RCL__STDATOMICS_HELPER_H_ #define RCL__STDATOMIC_HELPER_H_
#if !defined(WIN32) #if !defined(WIN32)
// #include <stdatomic.h> #include <stdatomic.h>
#define rcl_atomic_load(object, out) (out) = atomic_load(object)
#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \
(out) = atomic_compare_exchange_strong(object, expected, desired)
#define rcl_atomic_exchange(object, out, desired) (out) = atomic_exchange(object, desired)
#define rcl_atomic_store(object, desired) atomic_store(object, desired)
#else #else
#endif #endif
#endif // RCL__STDATOMICS_HELPER_H_ static inline bool
rcl_atomic_load_bool(atomic_bool * a_bool)
{
bool result;
rcl_atomic_load(a_bool, result);
return result;
}
static inline uint64_t
rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t)
{
uint64_t result;
rcl_atomic_load(a_uint64_t, result);
return result;
}
static inline uintptr_t
rcl_atomic_load_uintptr_t(atomic_uintptr_t * a_uintptr_t)
{
uintptr_t result;
rcl_atomic_load(a_uintptr_t, result);
return result;
}
static inline bool
rcl_atomic_compare_exchange_strong_uint_least64_t(
atomic_uint_least64_t * a_uint_least64_t, uint64_t * expected, uint64_t desired)
{
bool result;
rcl_atomic_compare_exchange_strong(a_uint_least64_t, result, expected, desired);
return result;
}
static inline uint64_t
rcl_atomic_exchange_uint64_t(atomic_uint_least64_t * a_uint64_t, uint64_t desired)
{
uint64_t result;
rcl_atomic_exchange(a_uint64_t, result, desired);
return result;
}
static inline uint64_t
rcl_atomic_exchange_uintptr_t(atomic_uintptr_t * a_uintptr_t, uintptr_t desired)
{
uintptr_t result;
rcl_atomic_exchange(a_uintptr_t, result, desired);
return result;
}
#endif // RCL__STDATOMIC_HELPER_H_

305
rcl/src/rcl/stdatomic_helper/win32/stdatomic.h
View file

@ -1,3 +1,17 @@
// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* /*
* An implementation of C11 stdatomic.h for Win32, part borrowed from FreeBSD * An implementation of C11 stdatomic.h for Win32, part borrowed from FreeBSD
* (original copyright follows), with major modifications for * (original copyright follows), with major modifications for
@ -42,18 +56,18 @@
* $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $ * $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $
*/ */
#ifndef _STDATOMIC_H_ #if !defined(WIN32)
#define _STDATOMIC_H_ #error "this stdatomic.h does not support your compiler"
#endif
#ifndef RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_
#define RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_
#include <Windows.h> #include <Windows.h>
#include <stddef.h> #include <stddef.h>
#include <stdint.h> #include <stdint.h>
#if !defined(WIN32)
#error "this stdatomic.h does not support your compiler"
#endif
// In MSVC, correct alignment of each type is already ensured. // In MSVC, correct alignment of each type is already ensured.
#define _Atomic(T) struct { T __val; } #define _Atomic(T) struct { T __val; }
@ -61,9 +75,9 @@
* 7.17.2 Initialization. * 7.17.2 Initialization.
*/ */
#define ATOMIC_VAR_INIT(value) { .__val = (value) } #define ATOMIC_VAR_INIT(value) {.__val = (value)}
#define atomic_init(obj, value) do { \ #define atomic_init(obj, value) do { \
(obj)->__val = (value); \ (obj)->__val = (value); \
} while (0) } while (0)
/* /*
@ -98,7 +112,8 @@
* atomic operations. * atomic operations.
*/ */
enum memory_order { enum memory_order
{
memory_order_relaxed = __ATOMIC_RELAXED, memory_order_relaxed = __ATOMIC_RELAXED,
memory_order_consume = __ATOMIC_CONSUME, memory_order_consume = __ATOMIC_CONSUME,
memory_order_acquire = __ATOMIC_ACQUIRE, memory_order_acquire = __ATOMIC_ACQUIRE,
@ -126,47 +141,47 @@ typedef enum memory_order memory_order;
* 7.17.6 Atomic integer types. * 7.17.6 Atomic integer types.
*/ */
typedef _Atomic(_Bool) atomic_bool; typedef _Atomic (_Bool) atomic_bool;
typedef _Atomic(char) atomic_char; typedef _Atomic (char) atomic_char;
typedef _Atomic(signed char) atomic_schar; typedef _Atomic (signed char) atomic_schar;
typedef _Atomic(unsigned char) atomic_uchar; typedef _Atomic (unsigned char) atomic_uchar;
typedef _Atomic(short) atomic_short; typedef _Atomic (short) atomic_short; // NOLINT
typedef _Atomic(unsigned short) atomic_ushort; typedef _Atomic (unsigned short) atomic_ushort; // NOLINT
typedef _Atomic(int) atomic_int; typedef _Atomic (int) atomic_int;
typedef _Atomic(unsigned int) atomic_uint; typedef _Atomic (unsigned int) atomic_uint;
typedef _Atomic(long) atomic_long; typedef _Atomic (long) atomic_long; // NOLINT
typedef _Atomic(unsigned long) atomic_ulong; typedef _Atomic (unsigned long) atomic_ulong; // NOLINT
typedef _Atomic(long long) atomic_llong; typedef _Atomic (long long) atomic_llong; // NOLINT
typedef _Atomic(unsigned long long) atomic_ullong; typedef _Atomic (unsigned long long) atomic_ullong; // NOLINT
#if 0 #if 0
typedef _Atomic(char16_t) atomic_char16_t; typedef _Atomic (char16_t) atomic_char16_t;
typedef _Atomic(char32_t) atomic_char32_t; typedef _Atomic (char32_t) atomic_char32_t;
typedef _Atomic(wchar_t) atomic_wchar_t; typedef _Atomic (wchar_t) atomic_wchar_t;
typedef _Atomic(int_least8_t) atomic_int_least8_t; typedef _Atomic (int_least8_t) atomic_int_least8_t;
typedef _Atomic(uint_least8_t) atomic_uint_least8_t; typedef _Atomic (uint_least8_t) atomic_uint_least8_t;
#endif #endif
typedef _Atomic(int_least16_t) atomic_int_least16_t; typedef _Atomic (int_least16_t) atomic_int_least16_t;
typedef _Atomic(uint_least16_t) atomic_uint_least16_t; typedef _Atomic (uint_least16_t) atomic_uint_least16_t;
typedef _Atomic(int_least32_t) atomic_int_least32_t; typedef _Atomic (int_least32_t) atomic_int_least32_t;
typedef _Atomic(uint_least32_t) atomic_uint_least32_t; typedef _Atomic (uint_least32_t) atomic_uint_least32_t;
typedef _Atomic(int_least64_t) atomic_int_least64_t; typedef _Atomic (int_least64_t) atomic_int_least64_t;
typedef _Atomic(uint_least64_t) atomic_uint_least64_t; typedef _Atomic (uint_least64_t) atomic_uint_least64_t;
#if 0 #if 0
typedef _Atomic(int_fast8_t) atomic_int_fast8_t; typedef _Atomic (int_fast8_t) atomic_int_fast8_t;
typedef _Atomic(uint_fast8_t) atomic_uint_fast8_t; typedef _Atomic (uint_fast8_t) atomic_uint_fast8_t;
#endif #endif
typedef _Atomic(int_fast16_t) atomic_int_fast16_t; typedef _Atomic (int_fast16_t) atomic_int_fast16_t;
typedef _Atomic(uint_fast16_t) atomic_uint_fast16_t; typedef _Atomic (uint_fast16_t) atomic_uint_fast16_t;
typedef _Atomic(int_fast32_t) atomic_int_fast32_t; typedef _Atomic (int_fast32_t) atomic_int_fast32_t;
typedef _Atomic(uint_fast32_t) atomic_uint_fast32_t; typedef _Atomic (uint_fast32_t) atomic_uint_fast32_t;
typedef _Atomic(int_fast64_t) atomic_int_fast64_t; typedef _Atomic (int_fast64_t) atomic_int_fast64_t;
typedef _Atomic(uint_fast64_t) atomic_uint_fast64_t; typedef _Atomic (uint_fast64_t) atomic_uint_fast64_t;
typedef _Atomic(intptr_t) atomic_intptr_t; typedef _Atomic (intptr_t) atomic_intptr_t;
typedef _Atomic(uintptr_t) atomic_uintptr_t; typedef _Atomic (uintptr_t) atomic_uintptr_t;
typedef _Atomic(size_t) atomic_size_t; typedef _Atomic (size_t) atomic_size_t;
typedef _Atomic(ptrdiff_t) atomic_ptrdiff_t; typedef _Atomic (ptrdiff_t) atomic_ptrdiff_t;
typedef _Atomic(intmax_t) atomic_intmax_t; typedef _Atomic (intmax_t) atomic_intmax_t;
typedef _Atomic(uintmax_t) atomic_uintmax_t; typedef _Atomic (uintmax_t) atomic_uintmax_t;
/* /*
* 7.17.7 Operations on atomic types. (pruned modified for Windows' crappy C compiler) * 7.17.7 Operations on atomic types. (pruned modified for Windows' crappy C compiler)
@ -174,128 +189,128 @@ typedef _Atomic(uintmax_t) atomic_uintmax_t;
#define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \ #define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedCompareExchange64((LONGLONG *) object, desired, expected); \ out = _InterlockedCompareExchange64((LONGLONG *) object, desired, expected); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedCompareExchange((LONG *) object, desired, expected); \ out = _InterlockedCompareExchange((LONG *) object, desired, expected); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedCompareExchange16((SHORT *) object, desired, expected); \ out = _InterlockedCompareExchange16((SHORT *) object, desired, expected); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \ #define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \
rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired)
#define rcl_win32_atomic_exchange((object), out, desired) \ #define rcl_win32_atomic_exchange((object), out, desired) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedExchange64((LONGLONG *) object, desired); \ out = _InterlockedExchange64((LONGLONG *) object, desired); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedExchange((LONG *) object, desired); \ out = _InterlockedExchange((LONG *) object, desired); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedExchange16((SHORT *) object, desired); \ out = _InterlockedExchange16((SHORT *) object, desired); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_fetch_add(object, out, operand) \ #define rcl_win32_atomic_fetch_add(object, out, operand) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedExchangeAdd64((LONGLONG *) object, operand); \ out = _InterlockedExchangeAdd64((LONGLONG *) object, operand); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedExchangeAdd((LONG *) object, operand); \ out = _InterlockedExchangeAdd((LONG *) object, operand); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedExchangeAdd16((SHORT *) object, operand); \ out = _InterlockedExchangeAdd16((SHORT *) object, operand); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_fetch_and(object, out, operand) \ #define rcl_win32_atomic_fetch_and(object, out, operand) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedAnd64((LONGLONG *) object, operand); \ out = _InterlockedAnd64((LONGLONG *) object, operand); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedAnd((LONG *) object, operand); \ out = _InterlockedAnd((LONG *) object, operand); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedAnd16((SHORT *) object, operand); \ out = _InterlockedAnd16((SHORT *) object, operand); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_fetch_or(object, out, operand) \ #define rcl_win32_atomic_fetch_or(object, out, operand) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedOr64((LONGLONG *) object, operand); \ out = _InterlockedOr64((LONGLONG *) object, operand); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedOr((LONG *) object, operand); \ out = _InterlockedOr((LONG *) object, operand); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedOr16((SHORT *) object, operand); \ out = _InterlockedOr16((SHORT *) object, operand); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_fetch_sub(object, out, operand) \ #define rcl_win32_atomic_fetch_sub(object, out, operand) \
rcl_win32_atomic_fetch_add(object, out, -(operand)) rcl_win32_atomic_fetch_add(object, out, -(operand))
#define rcl_win32_atomic_fetch_xor(object, out, operand) \ #define rcl_win32_atomic_fetch_xor(object, out, operand) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedXor64((LONGLONG *) object, operand); \ out = _InterlockedXor64((LONGLONG *) object, operand); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedXor((LONG *) object, operand); \ out = _InterlockedXor((LONG *) object, operand); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedXor16((SHORT *) object, operand); \ out = _InterlockedXor16((SHORT *) object, operand); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_load(object) \ #define rcl_win32_atomic_load(object) \
do { \ do { \
switch(sizeof(object)) { \ switch (sizeof(object)) { \
case sizeof(uint64_t): \ case sizeof(uint64_t): \
out = _InterlockedExchangeAdd64((LONGLONG *) object, 0); \ out = _InterlockedExchangeAdd64((LONGLONG *) object, 0); \
break; \ break; \
case sizeof(uint32_t): \ case sizeof(uint32_t): \
out = _InterlockedExchangeAdd((LONG *) object, 0); \ out = _InterlockedExchangeAdd((LONG *) object, 0); \
break; \ break; \
case sizeof(uint16_t): \ case sizeof(uint16_t): \
out = _InterlockedExchangeAdd16((SHORT *) object, 0); \ out = _InterlockedExchangeAdd16((SHORT *) object, 0); \
break; \ break; \
default: \ default: \
break; \ break; \
}; \ } \
} while(0) } while (0)
#define rcl_win32_atomic_store(object, desired) \ #define rcl_win32_atomic_store(object, desired) \
do { \ do { \
@ -322,4 +337,4 @@ typedef _Atomic(uintmax_t) atomic_uintmax_t;
// #define atomic_flag_test_and_set(object) \ // #define atomic_flag_test_and_set(object) \
// atomic_flag_test_and_set_explicit(object, memory_order_seq_cst) // atomic_flag_test_and_set_explicit(object, memory_order_seq_cst)
#endif /* !_STDATOMIC_H_ */ #endif // RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_

8
rcl/src/rcl/time_win32.c
View file

@ -24,10 +24,10 @@ extern "C"
#include "rcl/time.h" #include "rcl/time.h"
#include <stdatomic.h>
#include <windows.h> #include <windows.h>
#include "./common.h" #include "./common.h"
#include "./stdatomic_helper.h"
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0))
@ -63,7 +63,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now)
static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0);
rcl_time_t start = {0, 0}; rcl_time_t start = {0, 0};
// If start_ns (static/global) is 0, then set it up on the first call. // If start_ns (static/global) is 0, then set it up on the first call.
uint64_t start_ns_loaded = atomic_load(&start_ns); uint64_t start_ns_loaded = rcl_atomic_load(&start_ns);
if (start_ns_loaded == 0) { if (start_ns_loaded == 0) {
QueryPerformanceFrequency(&cpu_freq); QueryPerformanceFrequency(&cpu_freq);
if (cpu_freq.QuadPart == 0) { if (cpu_freq.QuadPart == 0) {
@ -86,7 +86,9 @@ rcl_system_time_point_now(rcl_system_time_point_t * now)
#endif #endif
start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd.
start.nsec = (start_li.LowPart % 10000000) * 100; start.nsec = (start_li.LowPart % 10000000) * 100;
if (atomic_compare_exchange_strong(&start_ns, 0, RCL_S_TO_NS(start.sec) + start.nsec)) { 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. // 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; init_cpu_time_global = init_cpu_time;
cpu_freq_global = cpu_freq; cpu_freq_global = cpu_freq;

38
rcl/src/rcl/timer.c
View file

@ -19,9 +19,8 @@ extern "C"
#include "rcl/timer.h" #include "rcl/timer.h"
#include <stdatomic.h>
#include "./common.h" #include "./common.h"
#include "./stdatomic_helper.h"
typedef struct rcl_timer_impl_t typedef struct rcl_timer_impl_t
{ {
@ -97,7 +96,7 @@ rcl_timer_call(rcl_timer_t * timer)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
if (atomic_load(&timer->impl->canceled)) { if (rcl_atomic_load_bool(&timer->impl->canceled)) {
RCL_SET_ERROR_MSG("timer is canceled"); RCL_SET_ERROR_MSG("timer is canceled");
return RCL_RET_TIMER_CANCELED; return RCL_RET_TIMER_CANCELED;
} }
@ -106,9 +105,11 @@ rcl_timer_call(rcl_timer_t * timer)
if (now_ret != RCL_RET_OK) { if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set. return now_ret; // rcl error state should already be set.
} }
uint64_t previous_ns = atomic_exchange(&timer->impl->last_call_time, now_steady.nanoseconds); uint64_t previous_ns =
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady.nanoseconds);
uint64_t since_last_call = now_steady.nanoseconds - previous_ns; uint64_t since_last_call = now_steady.nanoseconds - previous_ns;
rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback); rcl_timer_callback_t typed_callback =
(rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
typed_callback(timer, since_last_call); typed_callback(timer, since_last_call);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -124,7 +125,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready)
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
return ret; // rcl error state should already be set. return ret; // rcl error state should already be set.
} }
*is_ready = (time_until_next_call <= 0) && !atomic_load(&timer->impl->canceled); *is_ready = (time_until_next_call <= 0) && !rcl_atomic_load_bool(&timer->impl->canceled);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -139,8 +140,9 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
return ret; // rcl error state should already be set. return ret; // rcl error state should already be set.
} }
uint64_t period = atomic_load(&timer->impl->period); uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
*time_until_next_call = (atomic_load(&timer->impl->last_call_time) + period) - now.nanoseconds; *time_until_next_call =
(rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now.nanoseconds;
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -155,7 +157,8 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_si
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
return ret; // rcl error state should already be set. return ret; // rcl error state should already be set.
} }
*time_since_last_call = (now.nanoseconds - atomic_load(&timer->impl->last_call_time)); *time_since_last_call =
(now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time));
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -165,7 +168,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period)
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
*period = atomic_load(&timer->impl->period); *period = rcl_atomic_load_uint64_t(&timer->impl->period);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -175,7 +178,7 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
*old_period = atomic_exchange(&timer->impl->period, new_period); *old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -184,7 +187,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
return (rcl_timer_callback_t)atomic_load(&timer->impl->callback); return (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
} }
rcl_timer_callback_t rcl_timer_callback_t
@ -193,7 +196,8 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_
RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL); RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
return (rcl_timer_callback_t)atomic_exchange(&timer->impl->callback, (uintptr_t)new_callback); return (rcl_timer_callback_t)rcl_atomic_exchange_uintptr_t(
&timer->impl->callback, (uintptr_t)new_callback);
} }
rcl_ret_t rcl_ret_t
@ -201,7 +205,7 @@ rcl_timer_cancel(rcl_timer_t * timer)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
atomic_store(&timer->impl->canceled, true); rcl_atomic_store(&timer->impl->canceled, true);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -211,7 +215,7 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled)
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
*is_canceled = atomic_load(&timer->impl->canceled); *is_canceled = rcl_atomic_load_bool(&timer->impl->canceled);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -225,8 +229,8 @@ rcl_timer_reset(rcl_timer_t * timer)
if (now_ret != RCL_RET_OK) { if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set. return now_ret; // rcl error state should already be set.
} }
atomic_store(&timer->impl->last_call_time, now.nanoseconds); rcl_atomic_store(&timer->impl->last_call_time, now.nanoseconds);
atomic_store(&timer->impl->canceled, false); rcl_atomic_store(&timer->impl->canceled, false);
return RCL_RET_OK; return RCL_RET_OK;
} }

4
rcl/src/rcl/wait.c
View file

@ -20,14 +20,14 @@ extern "C"
#include "rcl/wait.h" #include "rcl/wait.h"
#include <assert.h> #include <assert.h>
#include <stdatomic.h>
#include <stdbool.h> #include <stdbool.h>
#include <string.h> #include <string.h>
#include "./common.h"
#include "./stdatomic_helper.h"
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#include "rcl/time.h" #include "rcl/time.h"
#include "rmw/rmw.h" #include "rmw/rmw.h"
#include "./common.h"
typedef struct rcl_wait_set_impl_t typedef struct rcl_wait_set_impl_t
{ {