diff --git a/src/common/pico_sync/CMakeLists.txt b/src/common/pico_sync/CMakeLists.txt index 05b969cd0..4e84e9aea 100644 --- a/src/common/pico_sync/CMakeLists.txt +++ b/src/common/pico_sync/CMakeLists.txt @@ -8,7 +8,7 @@ endif() if (NOT TARGET pico_sync) pico_add_impl_library(pico_sync) target_include_directories(pico_sync_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include) - pico_mirrored_target_link_libraries(pico_sync INTERFACE pico_sync_sem pico_sync_mutex pico_sync_critical_section pico_time hardware_sync) + pico_mirrored_target_link_libraries(pico_sync INTERFACE pico_sync_cond pico_sync_sem pico_sync_mutex pico_sync_critical_section pico_time hardware_sync) endif() @@ -19,6 +19,14 @@ if (NOT TARGET pico_sync_core) ) endif() +if (NOT TARGET pico_sync_cond) + pico_add_library(pico_sync_cond) + target_sources(pico_sync_cond INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/cond.c + ) + pico_mirrored_target_link_libraries(pico_sync_cond INTERFACE pico_sync_core) +endif() + if (NOT TARGET pico_sync_sem) pico_add_library(pico_sync_sem) target_sources(pico_sync_sem INTERFACE diff --git a/src/common/pico_sync/cond.c b/src/common/pico_sync/cond.c new file mode 100644 index 000000000..5dfb87064 --- /dev/null +++ b/src/common/pico_sync/cond.c @@ -0,0 +1,165 @@ +/* + * Copyright (c) 2022-2023 Paul Guyot + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "pico/cond.h" + +void cond_init(cond_t *cond) { + lock_init(&cond->core, next_striped_spin_lock_num()); + cond->waiter = LOCK_INVALID_OWNER_ID; + cond->broadcast_count = 0; + cond->signaled = false; + __mem_fence_release(); +} + +bool __time_critical_func(cond_wait_until)(cond_t *cond, mutex_t *mtx, absolute_time_t until) { + bool success = true; + lock_owner_id_t caller = lock_get_caller_owner_id(); + uint32_t save = save_and_disable_interrupts(); + // Acquire the mutex spin lock + spin_lock_unsafe_blocking(mtx->core.spin_lock); + assert(lock_is_owner_id_valid(mtx->owner)); + assert(caller == mtx->owner); + + // Mutex and cond spin locks can be the same as spin locks are attributed + // using `next_striped_spin_lock_num()`. To avoid any deadlock, we only + // acquire the condition variable spin lock if it is different from the + // mutex spin lock + bool same_spinlock = mtx->core.spin_lock == cond->core.spin_lock; + + // Acquire the condition variable spin_lock + if (!same_spinlock) { + spin_lock_unsafe_blocking(cond->core.spin_lock); + } + + // Release the mutex but without restoring interrupts and notify. + mtx->owner = LOCK_INVALID_OWNER_ID; + if (!same_spinlock) { + spin_unlock_unsafe(mtx->core.spin_lock); + } + + uint64_t current_broadcast = cond->broadcast_count; + + if (lock_is_owner_id_valid(cond->waiter)) { + // There is a valid owner of the condition variable: we are not the + // first waiter. + // First iteration: notify + lock_internal_spin_unlock_with_notify(&cond->core, save); + save = spin_lock_blocking(cond->core.spin_lock); + // Further iterations: wait + do { + if (!lock_is_owner_id_valid(cond->waiter)) { + break; + } + if (cond->broadcast_count != current_broadcast) { + break; + } + if (is_at_the_end_of_time(until)) { + lock_internal_spin_unlock_with_wait(&cond->core, save); + } else { + if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&cond->core, save, until)) { + // timed out + success = false; + break; + } + } + save = spin_lock_blocking(cond->core.spin_lock); + } while (true); + } else { + // Notify to finish release of mutex + __sev(); + } + + if (success && cond->broadcast_count == current_broadcast) { + cond->waiter = caller; + + // Wait for the signal + do { + if (cond->signaled) { + cond->waiter = LOCK_INVALID_OWNER_ID; + cond->signaled = false; + break; + } + if (is_at_the_end_of_time(until)) { + lock_internal_spin_unlock_with_wait(&cond->core, save); + } else { + if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&cond->core, save, until)) { + // timed out + cond->waiter = LOCK_INVALID_OWNER_ID; + success = false; + break; + } + } + save = spin_lock_blocking(cond->core.spin_lock); + } while (true); + } + + // We got the signal (or timed out) + // Acquire the mutex spin lock and release the core spin lock. + if (!same_spinlock) { + spin_lock_unsafe_blocking(mtx->core.spin_lock); + spin_unlock_unsafe(cond->core.spin_lock); + } + + if (lock_is_owner_id_valid(mtx->owner)) { + // Another core holds the mutex. + // First iteration: notify + lock_internal_spin_unlock_with_notify(&mtx->core, save); + save = spin_lock_blocking(mtx->core.spin_lock); + // Further iterations: wait + do { + if (!lock_is_owner_id_valid(mtx->owner)) { + break; + } + // We always wait for the mutex. + lock_internal_spin_unlock_with_wait(&mtx->core, save); + save = spin_lock_blocking(mtx->core.spin_lock); + } while (true); + } else { + // Notify to finish release of condition variable + __sev(); + } + + // Eventually hold the mutex. + mtx->owner = caller; + spin_unlock(mtx->core.spin_lock, save); + + return success; +} + +bool __time_critical_func(cond_wait_timeout_ms)(cond_t *cond, mutex_t *mtx, uint32_t timeout_ms) { + return cond_wait_until(cond, mtx, make_timeout_time_ms(timeout_ms)); +} + +bool __time_critical_func(cond_wait_timeout_us)(cond_t *cond, mutex_t *mtx, uint32_t timeout_us) { + return cond_wait_until(cond, mtx, make_timeout_time_us(timeout_us)); +} + +void __time_critical_func(cond_wait)(cond_t *cond, mutex_t *mtx) { + cond_wait_until(cond, mtx, at_the_end_of_time); +} + +void __time_critical_func(cond_signal)(cond_t *cond) { + uint32_t save = spin_lock_blocking(cond->core.spin_lock); + if (lock_is_owner_id_valid(cond->waiter)) { + // We have a waiter, we can signal. + cond->signaled = true; + lock_internal_spin_unlock_with_notify(&cond->core, save); + } else { + spin_unlock(cond->core.spin_lock, save); + } +} + +void __time_critical_func(cond_broadcast)(cond_t *cond) { + uint32_t save = spin_lock_blocking(cond->core.spin_lock); + if (lock_is_owner_id_valid(cond->waiter)) { + // We have a waiter, we can broadcast. + cond->signaled = true; + cond->broadcast_count++; + lock_internal_spin_unlock_with_notify(&cond->core, save); + } else { + spin_unlock(cond->core.spin_lock, save); + } +} diff --git a/src/common/pico_sync/include/pico/cond.h b/src/common/pico_sync/include/pico/cond.h new file mode 100644 index 000000000..4014d3617 --- /dev/null +++ b/src/common/pico_sync/include/pico/cond.h @@ -0,0 +1,121 @@ +/* + * Copyright (c) 2022-2023 Paul Guyot + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef _PLATFORM_COND_H +#define _PLATFORM_COND_H + +#include "pico/mutex.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** \file cond.h + * \defgroup cond cond + * \ingroup pico_sync + * \brief Condition variable API for non IRQ mutual exclusion between cores + * + * Condition variables complement mutexes by providing a way to atomically + * wait and release a held mutex. Then, the task on the other core can signal + * the variable, which ends the wait. Often, the other core would also hold + * the shared mutex, so the signaled task waits until the mutex is released. + * + * Condition variables can also be broadcast. + * + * In this implementation, it is not mandatory. The condition variables only + * work with non-recursive mutexes. + * + * Limitations of mutexes also apply to condition variables. See \ref mutex.h + */ + +typedef struct __packed_aligned +{ + lock_core_t core; + lock_owner_id_t waiter; + uint32_t broadcast_count; // Overflow is unlikely + bool signaled; +} cond_t; + +/*! \brief Initialize a condition variable structure + * \ingroup cond + * + * \param cv Pointer to condition variable structure + */ +void cond_init(cond_t *cv); + +/*! \brief Wait on a condition variable + * \ingroup cond + * + * Wait until a condition variable is signaled or broadcast. The mutex should + * be owned and is released atomically. It is reacquired when this function + * returns. + * + * \param cv Condition variable to wait on + * \param mtx Currently held mutex + */ +void cond_wait(cond_t *cv, mutex_t *mtx); + +/*! \brief Wait on a condition variable with a timeout. + * \ingroup cond + * + * Wait until a condition variable is signaled or broadcast until a given + * time. The mutex is released atomically and reacquired even if the wait + * timed out. + * + * \param cv Condition variable to wait on + * \param mtx Currently held mutex + * \param until The time after which to return if the condition variable was + * not signaled. + * \return true if the condition variable was signaled, false otherwise + */ +bool cond_wait_until(cond_t *cv, mutex_t *mtx, absolute_time_t until); + +/*! \brief Wait on a condition variable with a timeout. + * \ingroup cond + * + * Wait until a condition variable is signaled or broadcast until a given + * time. The mutex is released atomically and reacquired even if the wait + * timed out. + * + * \param cv Condition variable to wait on + * \param mtx Currently held mutex + * \param timeout_ms The timeout in milliseconds. + * \return true if the condition variable was signaled, false otherwise + */ +bool cond_wait_timeout_ms(cond_t *cv, mutex_t *mtx, uint32_t timeout_ms); + +/*! \brief Wait on a condition variable with a timeout. + * \ingroup cond + * + * Wait until a condition variable is signaled or broadcast until a given + * time. The mutex is released atomically and reacquired even if the wait + * timed out. + * + * \param cv Condition variable to wait on + * \param mtx Currently held mutex + * \param timeout_ms The timeout in microseconds. + * \return true if the condition variable was signaled, false otherwise + */ +bool cond_wait_timeout_us(cond_t *cv, mutex_t *mtx, uint32_t timeout_us); + +/*! \brief Signal on a condition variable and wake the waiter + * \ingroup cond + * + * \param cv Condition variable to signal + */ +void cond_signal(cond_t *cv); + +/*! \brief Broadcast a condition variable and wake every waiters + * \ingroup cond + * + * \param cv Condition variable to signal + */ +void cond_broadcast(cond_t *cv); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/src/common/pico_sync/include/pico/sync.h b/src/common/pico_sync/include/pico/sync.h index 041bfd7ff..f81469d50 100644 --- a/src/common/pico_sync/include/pico/sync.h +++ b/src/common/pico_sync/include/pico/sync.h @@ -15,5 +15,6 @@ #include "pico/sem.h" #include "pico/mutex.h" #include "pico/critical_section.h" +#include "pico/cond.h" #endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 207cb72e8..e6e74bea5 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -11,4 +11,5 @@ if (PICO_ON_DEVICE) add_subdirectory(hardware_pwm_test) add_subdirectory(cmsis_test) add_subdirectory(pico_sem_test) + add_subdirectory(pico_cond_test) endif() diff --git a/test/pico_cond_test/CMakeLists.txt b/test/pico_cond_test/CMakeLists.txt new file mode 100644 index 000000000..4dc2c0267 --- /dev/null +++ b/test/pico_cond_test/CMakeLists.txt @@ -0,0 +1,4 @@ +add_executable(pico_cond_test pico_cond_test.c) + +target_link_libraries(pico_cond_test PRIVATE pico_test pico_sync pico_multicore pico_stdlib ) +pico_add_extra_outputs(pico_cond_test) diff --git a/test/pico_cond_test/pico_cond_test.c b/test/pico_cond_test/pico_cond_test.c new file mode 100644 index 000000000..ba1617ff4 --- /dev/null +++ b/test/pico_cond_test/pico_cond_test.c @@ -0,0 +1,139 @@ +/** + * Copyright (c) 2022-2023 Paul Guyot + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include + +#include "pico/cond.h" +#include "pico/test.h" +#include "pico/multicore.h" +#include "pico/stdio.h" + +PICOTEST_MODULE_NAME("COND", "condition variable test"); + +static cond_t cond; +static mutex_t mutex; + +static volatile bool test_cond_wait_done; +static volatile bool test_cond_wait_ready; +static void test_cond_wait(void) { + busy_wait_ms(100); + mutex_enter_blocking(&mutex); + test_cond_wait_ready = true; + cond_wait(&cond, &mutex); + test_cond_wait_done = true; + mutex_exit(&mutex); +} + +static volatile bool test_cond_wait_timedout; +static void test_cond_wait_timeout(void) { + busy_wait_ms(100); + mutex_enter_blocking(&mutex); + test_cond_wait_ready = true; + test_cond_wait_timedout = cond_wait_timeout_ms(&cond, &mutex, 200); + test_cond_wait_done = true; + mutex_exit(&mutex); +} + +int main() { + stdio_init_all(); + mutex_init(&mutex); + cond_init(&cond); + + PICOTEST_START(); + + PICOTEST_START_SECTION("test cond wait / signal with mutex"); + test_cond_wait_ready = false; + test_cond_wait_done = false; + multicore_launch_core1(test_cond_wait); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_ready, "core1 is not ready"); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait for signal"); + mutex_enter_blocking(&mutex); + cond_signal(&cond); + busy_wait_ms(200); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait for mutex release"); + mutex_exit(&mutex); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_done, "core1 isn't done"); + multicore_reset_core1(); + PICOTEST_END_SECTION(); + + PICOTEST_START_SECTION("test cond wait / signal without mutex"); + test_cond_wait_ready = false; + test_cond_wait_done = false; + multicore_launch_core1(test_cond_wait); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_ready, "core1 is not ready"); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait for signal"); + cond_signal(&cond); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_done, "core1 isn't done"); + multicore_reset_core1(); + PICOTEST_END_SECTION(); + + PICOTEST_START_SECTION("test cond wait / broadcast with mutex"); + test_cond_wait_ready = false; + test_cond_wait_done = false; + multicore_launch_core1(test_cond_wait); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_ready, "core1 is not ready"); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait for signal"); + mutex_enter_blocking(&mutex); + cond_broadcast(&cond); + busy_wait_ms(200); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait for mutex release"); + mutex_exit(&mutex); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_done, "core1 isn't done"); + multicore_reset_core1(); + PICOTEST_END_SECTION(); + + PICOTEST_START_SECTION("test cond wait / broadcast without mutex"); + test_cond_wait_ready = false; + test_cond_wait_done = false; + multicore_launch_core1(test_cond_wait); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_ready, "core1 is not ready"); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait for signal"); + cond_broadcast(&cond); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_done, "core1 isn't done"); + multicore_reset_core1(); + PICOTEST_END_SECTION(); + + PICOTEST_START_SECTION("test cond wait with timeout and no signal"); + test_cond_wait_ready = false; + test_cond_wait_done = false; + test_cond_wait_timedout = false; + multicore_launch_core1(test_cond_wait_timeout); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_ready, "core1 is not ready"); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait"); + busy_wait_ms(200); + PICOTEST_CHECK(!test_cond_wait_timedout, "core1 did not time out"); + PICOTEST_CHECK(test_cond_wait_done, "core1 isn't done"); + multicore_reset_core1(); + PICOTEST_END_SECTION(); + + PICOTEST_START_SECTION("test cond wait with timeout and signal"); + test_cond_wait_ready = false; + test_cond_wait_done = false; + test_cond_wait_timedout = false; + multicore_launch_core1(test_cond_wait_timeout); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_ready, "core1 is not ready"); + PICOTEST_CHECK(!test_cond_wait_done, "core1 did not wait"); + mutex_enter_blocking(&mutex); + cond_signal(&cond); + mutex_exit(&mutex); + busy_wait_ms(200); + PICOTEST_CHECK(test_cond_wait_timedout, "core1 did time out"); + PICOTEST_CHECK(test_cond_wait_done, "core1 isn't done"); + multicore_reset_core1(); + PICOTEST_END_SECTION(); + + PICOTEST_END_TEST(); +}