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 01/15] 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 02/15] 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 03/15] 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)); } From c86205fa0d78b9c806a2ec8459c55ff05ad3d1f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 18 Nov 2024 00:57:19 +0100 Subject: [PATCH 04/15] Add timeout argument --- include/zenoh-pico/system/platform_common.h | 4 ++-- src/system/platform_common.c | 4 ++-- src/system/unix/system.c | 17 +++++++++++++++-- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/include/zenoh-pico/system/platform_common.h b/include/zenoh-pico/system/platform_common.h index cad62639a..81b2975fe 100644 --- a/include/zenoh-pico/system/platform_common.h +++ b/include/zenoh-pico/system/platform_common.h @@ -272,7 +272,7 @@ z_result_t _z_condvar_drop(_z_condvar_t *cv); z_result_t _z_condvar_signal(_z_condvar_t *cv); z_result_t _z_condvar_signal_all(_z_condvar_t *cv); z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m); -z_result_t _z_condvar_timedwait(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime); +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout); /** * Initializes a condition variable. @@ -322,7 +322,7 @@ z_result_t z_condvar_signal(z_loaned_condvar_t *cv); */ z_result_t z_condvar_wait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m); -z_result_t z_condvar_timedwait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime); +z_result_t z_condvar_wait_until(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime, bool *timeout); /*------------------ Sleep ------------------*/ /** diff --git a/src/system/platform_common.c b/src/system/platform_common.c index 9452d0a90..f6dc0bb6f 100644 --- a/src/system/platform_common.c +++ b/src/system/platform_common.c @@ -61,8 +61,8 @@ z_result_t z_condvar_drop(z_moved_condvar_t *cv) { return _z_condvar_drop(&cv->_ z_result_t z_condvar_signal(z_loaned_condvar_t *cv) { return _z_condvar_signal(cv); } z_result_t z_condvar_wait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m) { return _z_condvar_wait(cv, m); } -z_result_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); +z_result_t z_condvar_wait_until(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + return _z_condvar_wait_until(cv, m, abstime, timeout); } #endif // Z_FEATURE_MULTI_THREAD == 1 diff --git a/src/system/unix/system.c b/src/system/unix/system.c index 5ef1185c5..36a8112cf 100644 --- a/src/system/unix/system.c +++ b/src/system/unix/system.c @@ -12,6 +12,7 @@ // ZettaScale Zenoh Team, // +#include #include #include #include @@ -143,8 +144,20 @@ z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_co z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_cond_wait(cv, m)); } -z_result_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)); +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + int error = pthread_cond_timedwait(cv, m, abstime); + + if (error == ETIMEDOUT) { + if (timeout != NULL) { + *timeout = true; + } + return 0; + } + + if (timeout != NULL) { + *timeout = false; + } + _Z_CHECK_SYS_ERR(error); } #endif // Z_FEATURE_MULTI_THREAD == 1 From 8104433da4f11aeb560100857b996d36f1440aa9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 18 Nov 2024 01:24:51 +0100 Subject: [PATCH 05/15] Fix build on macos --- src/system/unix/system.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/system/unix/system.c b/src/system/unix/system.c index 36a8112cf..90912513e 100644 --- a/src/system/unix/system.c +++ b/src/system/unix/system.c @@ -132,7 +132,9 @@ z_result_t _z_mutex_unlock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_unloc z_result_t _z_condvar_init(_z_condvar_t *cv) { pthread_condattr_t attr; pthread_condattr_init(&attr); +#ifndef ZENOH_MACOS pthread_condattr_setclock(&attr, CLOCK_MONOTONIC); +#endif _Z_CHECK_SYS_ERR(pthread_cond_init(cv, &attr)); } From 566f7a3e686fc26f14015643f681b3190bfcb942 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 18 Nov 2024 12:33:32 +0100 Subject: [PATCH 06/15] Add missing docstrings --- include/zenoh-pico/system/platform_common.h | 40 +++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/include/zenoh-pico/system/platform_common.h b/include/zenoh-pico/system/platform_common.h index 81b2975fe..a0faff4a6 100644 --- a/include/zenoh-pico/system/platform_common.h +++ b/include/zenoh-pico/system/platform_common.h @@ -322,6 +322,23 @@ z_result_t z_condvar_signal(z_loaned_condvar_t *cv); */ z_result_t z_condvar_wait(z_loaned_condvar_t *cv, z_loaned_mutex_t *m); +/** + * Waits for a signal on the condition variable while holding a mutex until a specified time. + * + * The calling thread is blocked until the condition variable is signaled or the timeout occurs. + * The associated mutex must be locked by the calling thread, and it will be automatically unlocked while waiting. + * The `timeout` bool pointer should either be NULL or point to a valid memory, in which case the function will store a + * value indicating whether a timeout occurred. If NULL is passed in for `timeout`, it will not be set. + * + * Parameters: + * cv: Pointer to a :c:type:`z_loaned_condvar_t` on which to wait. + * m: Pointer to a :c:type:`z_loaned_mutex_t` that will be unlocked during the wait. + * abstime: Absolute end time. + * timeout: Whether a timeout occurred. + * + * Returns: + * ``0`` if the wait is successful, a negative value otherwise. + */ z_result_t z_condvar_wait_until(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime, bool *timeout); /*------------------ Sleep ------------------*/ @@ -397,8 +414,31 @@ unsigned long z_clock_elapsed_ms(z_clock_t *time); */ unsigned long z_clock_elapsed_s(z_clock_t *time); +/** + * Offsets the clock by a specified duration in microseconds. + * + * Parameters: + * clock: Pointer to a `z_clock_t` to offset. + * duration: The duration in microseconds. + */ void z_clock_advance_us(z_clock_t *clock, unsigned long duration); + +/** + * Offsets the clock by a specified duration in milliseconds. + * + * Parameters: + * clock: Pointer to a `z_clock_t` to offset. + * duration: The duration in milliseconds. + */ void z_clock_advance_ms(z_clock_t *clock, unsigned long duration); + +/** + * Offsets the clock by a specified duration in seconds. + * + * Parameters: + * clock: Pointer to a `z_clock_t` to offset. + * duration: The duration in seconds. + */ void z_clock_advance_s(z_clock_t *clock, unsigned long duration); /*------------------ Time ------------------*/ From 341d1f04d1955b2007cb37cb39eb842af30ed4ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 18 Nov 2024 13:47:29 +0100 Subject: [PATCH 07/15] Use POSIX implementation in arduino, espidf and zephyr ports --- src/system/arduino/esp32/system.c | 45 +++++++++++++++++++++++++++++-- src/system/espidf/system.c | 45 ++++++++++++++++++++++++++++++- src/system/zephyr/system.c | 45 ++++++++++++++++++++++++++++++- 3 files changed, 131 insertions(+), 4 deletions(-) diff --git a/src/system/arduino/esp32/system.c b/src/system/arduino/esp32/system.c index 2a6a257e6..d0c1c42d3 100644 --- a/src/system/arduino/esp32/system.c +++ b/src/system/arduino/esp32/system.c @@ -115,7 +115,12 @@ z_result_t _z_mutex_try_lock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_try z_result_t _z_mutex_unlock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_unlock(m)); } /*------------------ Condvar ------------------*/ -z_result_t _z_condvar_init(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_init(cv, NULL)); } +z_result_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)); +} z_result_t _z_condvar_drop(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_destroy(cv)); } @@ -123,7 +128,21 @@ z_result_t _z_condvar_signal(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_s z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_broadcast(cv)); } -z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_cond_wait(cv, m)); } +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + int error = pthread_cond_timedwait(cv, m, abstime); + + if (error == ETIMEDOUT) { + if (timeout != NULL) { + *timeout = true; + } + return 0; + } + + if (timeout != NULL) { + *timeout = false; + } + _Z_CHECK_SYS_ERR(error); +} #endif // Z_FEATURE_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ @@ -178,6 +197,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; diff --git a/src/system/espidf/system.c b/src/system/espidf/system.c index b66bb94c2..5acc72ff1 100644 --- a/src/system/espidf/system.c +++ b/src/system/espidf/system.c @@ -142,7 +142,12 @@ z_result_t _z_mutex_try_lock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_try z_result_t _z_mutex_unlock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_unlock(m)); } /*------------------ Condvar ------------------*/ -z_result_t _z_condvar_init(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_init(cv, NULL)); } +z_result_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)); +} z_result_t _z_condvar_drop(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_destroy(cv)); } @@ -151,6 +156,22 @@ z_result_t _z_condvar_signal(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_s z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_broadcast(cv)); } z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_cond_wait(cv, m)); } + +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + int error = pthread_cond_timedwait(cv, m, abstime); + + if (error == ETIMEDOUT) { + if (timeout != NULL) { + *timeout = true; + } + return 0; + } + + if (timeout != NULL) { + *timeout = false; + } + _Z_CHECK_SYS_ERR(error); +} #endif // Z_FEATURE_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ @@ -202,6 +223,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; diff --git a/src/system/zephyr/system.c b/src/system/zephyr/system.c index 7818c9993..6b36e01f3 100644 --- a/src/system/zephyr/system.c +++ b/src/system/zephyr/system.c @@ -108,7 +108,12 @@ z_result_t _z_mutex_try_lock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_try z_result_t _z_mutex_unlock(_z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_mutex_unlock(m)); } /*------------------ Condvar ------------------*/ -z_result_t _z_condvar_init(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_init(cv, 0)); } +z_result_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)); +} z_result_t _z_condvar_drop(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_destroy(cv)); } @@ -117,6 +122,22 @@ z_result_t _z_condvar_signal(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_s z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_broadcast(cv)); } z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { _Z_CHECK_SYS_ERR(pthread_cond_wait(cv, m)); } + +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + int error = pthread_cond_timedwait(cv, m, abstime); + + if (error == ETIMEDOUT) { + if (timeout != NULL) { + *timeout = true; + } + return 0; + } + + if (timeout != NULL) { + *timeout = false; + } + _Z_CHECK_SYS_ERR(error); +} #endif // Z_FEATURE_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ @@ -183,6 +204,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 e768f9d89578197a541cfbb27f1c82f9cbb334ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 18 Nov 2024 13:51:33 +0100 Subject: [PATCH 08/15] Add missing errno includes --- src/system/arduino/esp32/system.c | 1 + src/system/espidf/system.c | 1 + src/system/zephyr/system.c | 1 + 3 files changed, 3 insertions(+) diff --git a/src/system/arduino/esp32/system.c b/src/system/arduino/esp32/system.c index d0c1c42d3..5d1736626 100644 --- a/src/system/arduino/esp32/system.c +++ b/src/system/arduino/esp32/system.c @@ -15,6 +15,7 @@ #include #include #include +#include #include #include diff --git a/src/system/espidf/system.c b/src/system/espidf/system.c index 5acc72ff1..b6703178e 100644 --- a/src/system/espidf/system.c +++ b/src/system/espidf/system.c @@ -14,6 +14,7 @@ #include #include +#include #include #include diff --git a/src/system/zephyr/system.c b/src/system/zephyr/system.c index 6b36e01f3..f808d908a 100644 --- a/src/system/zephyr/system.c +++ b/src/system/zephyr/system.c @@ -20,6 +20,7 @@ #include #endif +#include #include #include #include From 600f3fe96ed73c1dd354002ffa97848b3e09a876 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 18 Nov 2024 13:54:00 +0100 Subject: [PATCH 09/15] Fix formatting --- src/system/arduino/esp32/system.c | 2 +- src/system/espidf/system.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/system/arduino/esp32/system.c b/src/system/arduino/esp32/system.c index 5d1736626..e14f189fe 100644 --- a/src/system/arduino/esp32/system.c +++ b/src/system/arduino/esp32/system.c @@ -13,9 +13,9 @@ // #include +#include #include #include -#include #include #include diff --git a/src/system/espidf/system.c b/src/system/espidf/system.c index b6703178e..2c95afd63 100644 --- a/src/system/espidf/system.c +++ b/src/system/espidf/system.c @@ -12,9 +12,9 @@ // ZettaScale Zenoh Team, // +#include #include #include -#include #include #include From 8ae9f4389f44e22b9507d1aba224dae54111e458 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 00:05:42 +0100 Subject: [PATCH 10/15] Implement z_clock_advance for mbed port --- src/system/mbed/system.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/system/mbed/system.cpp b/src/system/mbed/system.cpp index 9b8378ea0..872e4721d 100644 --- a/src/system/mbed/system.cpp +++ b/src/system/mbed/system.cpp @@ -167,6 +167,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 76582485de650c1c0315bcef43459b8799fd69ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 00:06:14 +0100 Subject: [PATCH 11/15] Implement z_clock_advance for freertos_plus_tcp port --- src/system/freertos_plus_tcp/system.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 0876240fe..81452c3d1 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -190,6 +190,15 @@ unsigned long z_clock_elapsed_ms(z_clock_t *instant) { return z_time_elapsed_ms( unsigned long z_clock_elapsed_s(z_clock_t *instant) { return z_clock_elapsed_ms(instant) / 1000; } +void z_clock_advance_us(z_clock_t *clock, unsigned long duration) { z_clock_advance_ms(clock, duration / 1000); } + +void z_clock_advance_ms(z_clock_t *clock, unsigned long duration) { + unsigned long ticks = pdMS_TO_TICKS(duration); + *clock += ticks; +} + +void z_clock_advance_s(z_clock_t *clock, unsigned long duration) { z_clock_advance_ms(clock, duration * 1000); } + /*------------------ Time ------------------*/ z_time_t z_time_now(void) { return xTaskGetTickCount(); } From 7c29df8ff230f4b48692a73f605cf8d7fd21b037 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 00:06:49 +0100 Subject: [PATCH 12/15] Implement z_clock_advance for windows port --- src/system/windows/system.c | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/system/windows/system.c b/src/system/windows/system.c index 51acd4146..acd83835b 100644 --- a/src/system/windows/system.c +++ b/src/system/windows/system.c @@ -237,6 +237,42 @@ unsigned long z_clock_elapsed_s(z_clock_t *instant) { return (unsigned long)elapsed; } +void z_clock_advance_us(z_clock_t *clock, unsigned long duration) { + LARGE_INTEGER frequency; + QueryPerformanceFrequency(&frequency); // ticks per second + + // Hardware not supporting QueryPerformanceFrequency + if (frequency.QuadPart == 0) { + return; + } + double ticks = (double)duration * frequency.QuadPart / 1000000.0; + clock->QuadPart += (LONGLONG)ticks; +} + +void z_clock_advance_ms(z_clock_t *clock, unsigned long duration) { + LARGE_INTEGER frequency; + QueryPerformanceFrequency(&frequency); // ticks per second + + // Hardware not supporting QueryPerformanceFrequency + if (frequency.QuadPart == 0) { + return; + } + double ticks = (double)duration * frequency.QuadPart / 1000.0; + clock->QuadPart += (LONGLONG)ticks; +} + +void z_clock_advance_s(z_clock_t *clock, unsigned long duration) { + LARGE_INTEGER frequency; + QueryPerformanceFrequency(&frequency); // ticks per second + + // Hardware not supporting QueryPerformanceFrequency + if (frequency.QuadPart == 0) { + return; + } + double ticks = (double)duration * frequency.QuadPart; + clock->QuadPart += (LONGLONG)ticks; +} + /*------------------ Time ------------------*/ z_time_t z_time_now(void) { z_time_t now; From 08448a181ed8349a3adf9fb9659ef4834e54093e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 00:21:45 +0100 Subject: [PATCH 13/15] Implement z_condvar_wait_until in windows port --- src/system/windows/system.c | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/src/system/windows/system.c b/src/system/windows/system.c index acd83835b..f7a2b8761 100644 --- a/src/system/windows/system.c +++ b/src/system/windows/system.c @@ -158,6 +158,37 @@ z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { SleepConditionVariableSRW(cv, m, INFINITE, 0); return ret; } + +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + z_clock_t now = z_clock_now(); + LARGE_INTEGER frequency; + QueryPerformanceFrequency(&frequency); // ticks per second + + // Hardware not supporting QueryPerformanceFrequency + if (frequency.QuadPart == 0) { + return _Z_ERR_GENERIC; + } + + double remaining = (double)(abstime->QuadPart - now.QuadPart) / frequency.QuadPart * 1000.0; + DWORD block_duration = remaining > 0.0 ? (DWORD)remaining : 0; + + z_result_t ret = _Z_RES_OK; + if (SleepConditionVariableSRW(cv, m, block_duration, 0) == 0) { + if (GetLastError() == ERROR_TIMEOUT) { + if (timeout != NULL) { + *timeout = true; + } + return _Z_RES_OK; + } else { + ret = _Z_ERR_GENERIC; + } + } + + if (timeout != NULL) { + *timeout = false; + } + return ret; +} #endif // Z_FEATURE_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ From 6925cd559c784ecb335c7fb08f43903b4b4ff97e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 10 Dec 2024 05:20:40 +0100 Subject: [PATCH 14/15] Implement z_condvar_wait_until for freertos_plus_tcp and mbed ports --- src/system/freertos_plus_tcp/system.c | 32 ++++++++++++++++++++++++++ src/system/mbed/system.cpp | 33 +++++++++++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 726f9236c..f4f1606b2 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -233,6 +233,38 @@ z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return _Z_RES_OK; } + +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + if (!cv || !m) { + return _Z_ERR_GENERIC; + } + + TickType_t now = xTaskGetTickCount(); + TickType_t target_time = *abstime; + TickType_t block_duration = (target_time > now) ? (target_time - now) : 0; + + xSemaphoreTake(cv->mutex, portMAX_DELAY); + cv->waiters++; + xSemaphoreGive(cv->mutex); + + _z_mutex_unlock(m); + + bool timed_out = xSemaphoreTake(cv->sem, block_duration) == pdFALSE; + + _z_mutex_lock(m); + + if (timed_out) { + xSemaphoreTake(cv->mutex, portMAX_DELAY); + cv->waiters--; + xSemaphoreGive(cv->mutex); + } + + if (timeout != NULL) { + *timeout = timed_out; + } + + return _Z_RES_OK; +} #endif // Z_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ diff --git a/src/system/mbed/system.cpp b/src/system/mbed/system.cpp index 627293ce0..425fb1e4d 100644 --- a/src/system/mbed/system.cpp +++ b/src/system/mbed/system.cpp @@ -171,6 +171,39 @@ z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return _Z_RES_OK; } + +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + if (!cv || !m) { + return _Z_ERR_GENERIC; + } + + auto &cond_var = *(condvar *)*cv; + + auto target_time = + Kernel::Clock::time_point(Kernel::Clock::duration(abstime->tv_sec * 1000LL + abstime->tv_nsec / 1000000)); + + cond_var.mutex.lock(); + cond_var.waiters++; + cond_var.mutex.unlock(); + + _z_mutex_unlock(m); + + bool timed_out = cond_var.sem.try_acquire_until(target_time) == false; + + _z_mutex_lock(m); + + if (timed_out) { + cond_var.mutex.lock(); + cond_var.waiters--; + cond_var.mutex.unlock(); + } + + if (timeout != NULL) { + *timeout = timed_out; + } + + return _Z_RES_OK; +} #endif // Z_FEATURE_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ From 4890d429e051516e380069286d94012f39812911 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 10 Dec 2024 05:32:35 +0100 Subject: [PATCH 15/15] Implement z_condvar_wait_until for rpi_pico port --- src/system/rpi_pico/system.c | 54 ++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/src/system/rpi_pico/system.c b/src/system/rpi_pico/system.c index eb7f6c4cc..3f825fefe 100644 --- a/src/system/rpi_pico/system.c +++ b/src/system/rpi_pico/system.c @@ -206,6 +206,38 @@ z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return _z_mutex_lock(m); } + +z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime, bool *timeout) { + if (!cv || !m) { + return _Z_ERR_GENERIC; + } + + TickType_t now = xTaskGetTickCount(); + TickType_t target_time = pdMS_TO_TICKS(abstime->tv_sec * 1000 + abstime->tv_nsec / 1000000); + TickType_t block_duration = (target_time > now) ? (target_time - now) : 0; + + xSemaphoreTake(cv->mutex, portMAX_DELAY); + cv->waiters++; + xSemaphoreGive(cv->mutex); + + _z_mutex_unlock(m); + + bool timed_out = xSemaphoreTake(cv->sem, block_duration) == pdFALSE; + + _z_mutex_lock(m); + + if (timed_out) { + xSemaphoreTake(cv->mutex, portMAX_DELAY); + cv->waiters--; + xSemaphoreGive(cv->mutex); + } + + if (timeout != NULL) { + *timeout = timed_out; + } + + return _Z_RES_OK; +} #endif // Z_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ @@ -263,6 +295,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;