Implement condition variables for Mbed and FreeRTOS-Plus-TCP ports (#821

) * Fix Mbed condition variable implementation * Implement condition variable in FreeRTOS-Plus-TCP port * Use counting semaphore condvar implementation in FreeRTOS-Plus-TCP port Co-Authored-By: Alexander Bushnev <[email protected]> * Use counting semaphore condvar implementation in Mbed port * Use normal mutex instead of recursive * Fix typo in mbed port --------- Co-authored-by: Alexander Bushnev <[email protected]>
eclipse-zenoh · Dec 9, 2024 · 2f7880a · 2f7880a
1 parent 06fb1ef
commit 2f7880a
Show file tree

Hide file tree

Showing 3 changed files with 149 additions and 16 deletions.
diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h
@@ -40,7 +40,15 @@ typedef struct {
 } _z_task_t;
 
 typedef SemaphoreHandle_t _z_mutex_t;
-typedef void *_z_condvar_t;
+typedef struct {
+    SemaphoreHandle_t mutex;
+    SemaphoreHandle_t sem;
+    int waiters;
+#if (configSUPPORT_STATIC_ALLOCATION == 1)
+    StaticSemaphore_t mutex_buffer;
+    StaticSemaphore_t sem_buffer;
+#endif /* SUPPORT_STATIC_ALLOCATION */
+} _z_condvar_t;
 #endif  // Z_MULTI_THREAD == 1
 
 typedef TickType_t z_clock_t;

diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c
@@ -157,12 +157,82 @@ z_result_t _z_mutex_try_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m,
 z_result_t _z_mutex_unlock(_z_mutex_t *m) { return xSemaphoreGiveRecursive(*m) == pdTRUE ? 0 : -1; }
 
 /*------------------ CondVar ------------------*/
-// Condition variables not supported in FreeRTOS
-z_result_t _z_condvar_init(_z_condvar_t *cv) { return -1; }
-z_result_t _z_condvar_drop(_z_condvar_t *cv) { return -1; }
-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_init(_z_condvar_t *cv) {
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+#if (configSUPPORT_STATIC_ALLOCATION == 1)
+    cv->mutex = xSemaphoreCreateMutexStatic(&cv->mutex_buffer);
+    cv->sem = xSemaphoreCreateCountingStatic((UBaseType_t)(~0), 0, &cv->sem_buffer);
+#else
+    cv->mutex = xSemaphoreCreateMutex();
+    cv->sem = xSemaphoreCreateCounting((UBaseType_t)(~0), 0);
+#endif /* SUPPORT_STATIC_ALLOCATION */
+    cv->waiters = 0;
+
+    if (!cv->mutex || !cv->sem) {
+        return _Z_ERR_GENERIC;
+    }
+    return _Z_RES_OK;
+}
+
+z_result_t _z_condvar_drop(_z_condvar_t *cv) {
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+    vSemaphoreDelete(cv->sem);
+    vSemaphoreDelete(cv->mutex);
+    return _Z_RES_OK;
+}
+
+z_result_t _z_condvar_signal(_z_condvar_t *cv) {
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+    xSemaphoreTake(cv->mutex, portMAX_DELAY);
+    if (cv->waiters > 0) {
+        xSemaphoreGive(cv->sem);
+        cv->waiters--;
+    }
+    xSemaphoreGive(cv->mutex);
+
+    return _Z_RES_OK;
+}
+
+z_result_t _z_condvar_signal_all(_z_condvar_t *cv) {
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+    xSemaphoreTake(cv->mutex, portMAX_DELAY);
+    while (cv->waiters > 0) {
+        xSemaphoreGive(cv->sem);
+        cv->waiters--;
+    }
+    xSemaphoreGive(cv->mutex);
+
+    return _Z_RES_OK;
+}
+
+z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) {
+    if (!cv || !m) {
+        return _Z_ERR_GENERIC;
+    }
+
+    xSemaphoreTake(cv->mutex, portMAX_DELAY);
+    cv->waiters++;
+    xSemaphoreGive(cv->mutex);
+
+    _z_mutex_unlock(m);
+
+    xSemaphoreTake(cv->sem, portMAX_DELAY);
+
+    _z_mutex_lock(m);
+
+    return _Z_RES_OK;
+}
 #endif  // Z_MULTI_THREAD == 1
 
 /*------------------ Sleep ------------------*/

diff --git a/src/system/mbed/system.cpp b/src/system/mbed/system.cpp
@@ -94,27 +94,82 @@ z_result_t _z_mutex_unlock(_z_mutex_t *m) {
 }
 
 /*------------------ Condvar ------------------*/
-z_result_t _z_condvar_init(_z_condvar_t *cv) { return 0; }
+struct condvar {
+    Mutex mutex;
+    Semaphore sem{0};
+    int waiters{0};
+};
+
+z_result_t _z_condvar_init(_z_condvar_t *cv) {
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+    *cv = new condvar();
+    return 0;
+}
 
 z_result_t _z_condvar_drop(_z_condvar_t *cv) {
-    delete ((ConditionVariable *)*cv);
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+    delete ((condvar *)*cv);
     return 0;
 }
 
 z_result_t _z_condvar_signal(_z_condvar_t *cv) {
-    ((ConditionVariable *)*cv)->notify_one();
-    return 0;
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+    auto &cond_var = *(condvar *)*cv;
+
+    cond_var.mutex.lock();
+    if (cond_var.waiters > 0) {
+        cond_var.sem.release();
+        cond_var.waiters--;
+    }
+    cond_var.mutex.unlock();
+
+    return _Z_RES_OK;
 }
 
 z_result_t _z_condvar_signal_all(_z_condvar_t *cv) {
-    ((ConditionVariable *)*cv)->notify_all();
-    return 0;
+    if (!cv) {
+        return _Z_ERR_GENERIC;
+    }
+
+    auto &cond_var = *(condvar *)*cv;
+
+    cond_var.mutex.lock();
+    while (cond_var.waiters > 0) {
+        cond_var.sem.release();
+        cond_var.waiters--;
+    }
+    cond_var.mutex.unlock();
+
+    return _Z_RES_OK;
 }
 
 z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) {
-    *cv = new ConditionVariable(*((Mutex *)*m));
-    ((ConditionVariable *)*cv)->wait();
-    return 0;
+    if (!cv || !m) {
+        return _Z_ERR_GENERIC;
+    }
+
+    auto &cond_var = *(condvar *)*cv;
+
+    cond_var.mutex.lock();
+    cond_var.waiters++;
+    cond_var.mutex.unlock();
+
+    _z_mutex_unlock(m);
+
+    cond_var.sem.acquire();
+
+    _z_mutex_lock(m);
+
+    return _Z_RES_OK;
 }
 #endif  // Z_FEATURE_MULTI_THREAD == 1