From 5b9b415724588edfe954342feaf31a25ca6d0ecc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 16 Sep 2024 01:51:06 +0200 Subject: [PATCH 1/3] Add z_clock_advance functions --- include/zenoh-pico/system/platform-common.h | 3 +++ src/system/unix/system.c | 22 +++++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/include/zenoh-pico/system/platform-common.h b/include/zenoh-pico/system/platform-common.h index 15f2440aa..68793efc3 100644 --- a/include/zenoh-pico/system/platform-common.h +++ b/include/zenoh-pico/system/platform-common.h @@ -140,6 +140,9 @@ z_clock_t z_clock_now(void); unsigned long z_clock_elapsed_us(z_clock_t *time); unsigned long z_clock_elapsed_ms(z_clock_t *time); unsigned long z_clock_elapsed_s(z_clock_t *time); +void z_clock_advance_us(z_clock_t *clock, unsigned long duration); +void z_clock_advance_ms(z_clock_t *clock, unsigned long duration); +void z_clock_advance_s(z_clock_t *clock, unsigned long duration); /*------------------ Time ------------------*/ z_time_t z_time_now(void); diff --git a/src/system/unix/system.c b/src/system/unix/system.c index e38233daa..661ae3cdc 100644 --- a/src/system/unix/system.c +++ b/src/system/unix/system.c @@ -189,6 +189,28 @@ unsigned long z_clock_elapsed_s(z_clock_t *instant) { return elapsed; } +void z_clock_advance_us(z_clock_t *clock, unsigned long duration) { + clock->tv_sec += duration / 1000000; + clock->tv_nsec += (duration % 1000000) * 1000; + + if (clock->tv_nsec >= 1000000000) { + clock->tv_sec += 1; + clock->tv_nsec -= 1000000000; + } +} + +void z_clock_advance_ms(z_clock_t *clock, unsigned long duration) { + clock->tv_sec += duration / 1000; + clock->tv_nsec += (duration % 1000) * 1000000; + + if (clock->tv_nsec >= 1000000000) { + clock->tv_sec += 1; + clock->tv_nsec -= 1000000000; + } +} + +void z_clock_advance_s(z_clock_t *clock, unsigned long duration) { clock->tv_sec += duration; } + /*------------------ Time ------------------*/ z_time_t z_time_now(void) { z_time_t now; From 7049dcc4d29c3b00dce75d6347c58c28a07a7009 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 16 Sep 2024 01:56:15 +0200 Subject: [PATCH 2/3] Add z_condvar_timedwait function --- include/zenoh-pico/system/platform-common.h | 2 ++ src/system/platform-common.c | 3 +++ src/system/unix/system.c | 4 ++++ 3 files changed, 9 insertions(+) diff --git a/include/zenoh-pico/system/platform-common.h b/include/zenoh-pico/system/platform-common.h index 68793efc3..780a08308 100644 --- a/include/zenoh-pico/system/platform-common.h +++ b/include/zenoh-pico/system/platform-common.h @@ -120,6 +120,7 @@ int8_t _z_condvar_drop(_z_condvar_t *cv); int8_t _z_condvar_signal(_z_condvar_t *cv); int8_t _z_condvar_signal_all(_z_condvar_t *cv); int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m); +int8_t _z_condvar_timedwait(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime); _Z_OWNED_TYPE_VALUE(_z_condvar_t, condvar) _Z_OWNED_FUNCTIONS_SYSTEM_DEF(condvar) @@ -129,6 +130,7 @@ int8_t z_condvar_drop(z_moved_condvar_t *cv); int8_t z_condvar_signal(z_loaned_condvar_t *cv); int8_t z_condvar_wait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m); +int8_t z_condvar_timedwait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime); /*------------------ Sleep ------------------*/ int z_sleep_us(size_t time); diff --git a/src/system/platform-common.c b/src/system/platform-common.c index ab1a9d2e4..fc2060b98 100644 --- a/src/system/platform-common.c +++ b/src/system/platform-common.c @@ -52,5 +52,8 @@ int8_t z_condvar_drop(z_moved_condvar_t *cv) { return _z_condvar_drop(&cv->_this int8_t z_condvar_signal(z_loaned_condvar_t *cv) { return _z_condvar_signal(cv); } int8_t z_condvar_wait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m) { return _z_condvar_wait(cv, m); } +int8_t z_condvar_timedwait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime) { + return _z_condvar_timedwait(cv, m, abstime); +} #endif // Z_FEATURE_MULTI_THREAD == 1 diff --git a/src/system/unix/system.c b/src/system/unix/system.c index 661ae3cdc..4d273aed7 100644 --- a/src/system/unix/system.c +++ b/src/system/unix/system.c @@ -136,6 +136,10 @@ int8_t _z_condvar_signal(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_signa int8_t _z_condvar_signal_all(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_broadcast(cv)); } int8_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_cond_wait(cv, m)); } + +int8_t _z_condvar_timedwait(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime) { + _Z_CHECK_SYS_ERR(pthread_cond_timedwait(cv, m, abstime)); +} #endif // Z_FEATURE_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ From 72d794f86e5e966c16678108824a65c162061488 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 16 Sep 2024 01:56:58 +0200 Subject: [PATCH 3/3] Use monotonic clock in unix port of condvar --- src/system/unix/system.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/system/unix/system.c b/src/system/unix/system.c index 4d273aed7..78caf0883 100644 --- a/src/system/unix/system.c +++ b/src/system/unix/system.c @@ -127,7 +127,12 @@ int8_t _z_mutex_try_lock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_trylock int8_t _z_mutex_unlock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_unlock(m)); } /*------------------ Condvar ------------------*/ -int8_t _z_condvar_init(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_init(cv, 0)); } +int8_t _z_condvar_init(_z_condvar_t *cv) { + pthread_condattr_t attr; + pthread_condattr_init(&attr); + pthread_condattr_setclock(&attr, CLOCK_MONOTONIC); + _Z_CHECK_SYS_ERR(pthread_cond_init(cv, &attr)); +} int8_t _z_condvar_drop(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_destroy(cv)); }