From 2f7880af76a6bcb31e5ddc59a95893be389c93b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 9 Dec 2024 14:29:09 +0100 Subject: [PATCH] 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 * Use counting semaphore condvar implementation in Mbed port * Use normal mutex instead of recursive * Fix typo in mbed port --------- Co-authored-by: Alexander Bushnev --- .../system/platform/freertos_plus_tcp.h | 10 ++- src/system/freertos_plus_tcp/system.c | 82 +++++++++++++++++-- src/system/mbed/system.cpp | 73 +++++++++++++++-- 3 files changed, 149 insertions(+), 16 deletions(-) diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h index 31a2dfc83..97d6c1b10 100644 --- 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 index 0876240fe..395e6bc59 100644 --- 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 index 9b8378ea0..92ed064ac 100644 --- 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