Add condition variables to pico_sync (fix raspberrypi#1093)

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

src/common/pico_sync/cond.c

@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2022-2023 Paul Guyot <pguyot@kallisys.net>
+ *
+ * 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);
+    }
+}

src/common/pico_sync/include/pico/cond.h

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2022-2023 Paul Guyot <pguyot@kallisys.net>
+ *
+ * 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

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

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()

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)