Support waiting on condition variable until a specific time point (#665)

* Add z_clock_advance functions

* Add z_condvar_timedwait function

* Use monotonic clock in unix port of condvar

* Add timeout argument

* Fix build on macos

* Add missing docstrings

* Use POSIX implementation in arduino, espidf and zephyr ports

* Add missing errno includes

* Fix formatting

* Implement z_clock_advance for mbed port

* Implement z_clock_advance for freertos_plus_tcp port

* Implement z_clock_advance for windows port

* Implement z_condvar_wait_until in windows port

* Implement z_condvar_wait_until for freertos_plus_tcp and mbed ports

* Implement z_condvar_wait_until for rpi_pico port

* Add opencv and flipper clock advance implementations

* Add emscripten implementation

* Return a timeout result code instead of setting timeout variable

include/zenoh-pico/system/common/platform.h

@@ -274,6 +274,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_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime);
 
 /**
  * Initializes a condition variable.
@@ -323,6 +324,22 @@ 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.
+ *
+ * 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.
+ *
+ * Returns:
+ *   ``0`` if the wait is successful, ``Z_ETIMEDOUT`` if a timeout occurred, other 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);
+
 /*------------------ Sleep ------------------*/
 /**
  * Suspends execution for a specified amount of time in microseconds.
@@ -396,6 +413,33 @@ 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 ------------------*/
 
 /**

include/zenoh-pico/utils/result.h

@@ -86,6 +86,7 @@ typedef enum {
     _Z_ERR_OVERFLOW = -74,
     _Z_ERR_SESSION_CLOSED = -73,
     Z_EDESERIALIZE = -72,
+    Z_ETIMEDOUT = -71,
 
     _Z_ERR_GENERIC = -128
 } _z_res_t;

src/system/arduino/esp32/system.c

@@ -13,6 +13,7 @@
 //
 
 #include <Arduino.h>
+#include <errno.h>
 #include <esp_heap_caps.h>
 #include <esp_random.h>
 #include <stddef.h>
@@ -114,15 +115,28 @@ 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)); }
 
 z_result_t _z_condvar_signal(_z_condvar_t *cv) { _Z_CHECK_SYS_ERR(pthread_cond_signal(cv)); }
 
 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) {
+    int error = pthread_cond_timedwait(cv, m, abstime);
+
+    if (error == ETIMEDOUT) {
+        return Z_ETIMEDOUT;
+    }
+
+    _Z_CHECK_SYS_ERR(error);
+}
 #endif  // Z_FEATURE_MULTI_THREAD == 1
 
 /*------------------ Sleep ------------------*/
@@ -177,6 +191,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;

src/system/arduino/opencr/system.c

@@ -97,6 +97,7 @@ z_result_t _z_condvar_signal(_z_condvar_t *cv) { return -1; }
 z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { return -1; }
 
 z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return -1; }
+z_result_t _z_condvar_wait_until(_z_condvar_t *cv, _z_mutex_t *m, const z_clock_t *abstime) { return -1; }
 #endif  // Z_FEATURE_MULTI_THREAD == 1
 
 /*------------------ Sleep ------------------*/
@@ -160,6 +161,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;

src/system/common/platform.c

@@ -61,5 +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_wait_until(z_loaned_condvar_t *cv, z_loaned_mutex_t *m, const z_clock_t *abstime) {
+    return _z_condvar_wait_until(cv, m, abstime);
+}
 
 #endif  // Z_FEATURE_MULTI_THREAD == 1

src/system/emscripten/system.c

@@ -13,6 +13,7 @@
 //
 
 #include <emscripten/html5.h>
+#include <errno.h>
 #include <pthread.h>
 #include <stddef.h>
 #include <stdio.h>
@@ -69,7 +70,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)); }
 
@@ -78,6 +84,20 @@ 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) {
+    struct timespec ts;
+    ts.tv_sec = (time_t)(*abstime / 1000);
+    ts.tv_nsec = (long)((*abstime - (ts.tv_sec * 1000)) * 1000000);
+
+    int error = pthread_cond_timedwait(cv, m, &ts);
+
+    if (error == ETIMEDOUT) {
+        return Z_ETIMEDOUT;
+    }
+
+    _Z_CHECK_SYS_ERR(error);
+}
 #endif  // Z_FEATURE_MULTI_THREAD == 1
 
 /*------------------ Sleep ------------------*/
@@ -111,7 +131,13 @@ unsigned long z_clock_elapsed_us(z_clock_t *instant) { return z_clock_elapsed_ms
 
 unsigned long z_clock_elapsed_ms(z_clock_t *instant) { return z_time_elapsed_ms(instant); }
 
-unsigned long z_clock_elapsed_s(z_clock_t *instant) { return z_time_elapsed_ms(instant) * 1000; }
+unsigned long z_clock_elapsed_s(z_clock_t *instant) { return z_time_elapsed_ms(instant) / 1000; }
+
+void z_clock_advance_us(z_clock_t *clock, unsigned long duration) { *clock += (double)(duration / 1000); }
+
+void z_clock_advance_ms(z_clock_t *clock, unsigned long duration) { *clock += (double)duration; }
+
+void z_clock_advance_s(z_clock_t *clock, unsigned long duration) { *clock += (double)(duration * 1000); }
 
 /*------------------ Time ------------------*/
 z_time_t z_time_now(void) { return emscripten_get_now(); }

src/system/espidf/system.c

@@ -12,6 +12,7 @@
 //   ZettaScale Zenoh Team, <[email protected]>
 //
 
+#include <errno.h>
 #include <esp_heap_caps.h>
 #include <esp_random.h>
 #include <stddef.h>
@@ -142,7 +143,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 +157,16 @@ 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) {
+    int error = pthread_cond_timedwait(cv, m, abstime);
+
+    if (error == ETIMEDOUT) {
+        return Z_ETIMEDOUT;
+    }
+
+    _Z_CHECK_SYS_ERR(error);
+}
 #endif  // Z_FEATURE_MULTI_THREAD == 1
 
 /*------------------ Sleep ------------------*/
@@ -202,6 +218,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;

src/system/flipper/system.c

@@ -153,6 +153,8 @@ z_result_t _z_condvar_signal_all(_z_condvar_t* cv) { return -1; }
 
 z_result_t _z_condvar_wait(_z_condvar_t* cv, _z_mutex_t* m) { return -1; }
 
+z_result_t _z_condvar_wait_until(_z_condvar_t* cv, _z_mutex_t* m, const z_clock_t* abstime) { return -1; }
+
 /*------------------ Sleep ------------------*/
 z_result_t z_sleep_us(size_t time) {
     furi_delay_us(time);
@@ -214,6 +216,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;