Skip to content

Commit 58fa5fb

Browse files
pguyotpguyot
committed
Add condition variables to pico_sync (fix #1093)
1 parent e87f11b commit 58fa5fb

File tree

7 files changed

+447
-1
lines changed

7 files changed

+447
-1
lines changed

‎src/common/pico_sync/CMakeLists.txt‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ endif()
88
if (NOT TARGET pico_sync)
99
pico_add_impl_library(pico_sync)
1010
target_include_directories(pico_sync_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
11-
pico_mirrored_target_link_libraries(pico_sync INTERFACE pico_sync_sem pico_sync_mutex pico_sync_critical_section pico_time hardware_sync)
11+
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)
1212
endif()
1313

1414

@@ -19,6 +19,14 @@ if (NOT TARGET pico_sync_core)
1919
)
2020
endif()
2121

22+
if (NOT TARGET pico_sync_cond)
23+
pico_add_library(pico_sync_cond)
24+
target_sources(pico_sync_cond INTERFACE
25+
${CMAKE_CURRENT_LIST_DIR}/cond.c
26+
)
27+
pico_mirrored_target_link_libraries(pico_sync_cond INTERFACE pico_sync_core)
28+
endif()
29+
2230
if (NOT TARGET pico_sync_sem)
2331
pico_add_library(pico_sync_sem)
2432
target_sources(pico_sync_sem INTERFACE

‎src/common/pico_sync/cond.c‎

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
/*
2+
* Copyright (c) 2022-2023 Paul Guyot <[email protected]>
3+
*
4+
* SPDX-License-Identifier: BSD-3-Clause
5+
*/
6+
7+
#include "pico/cond.h"
8+
9+
void cond_init(cond_t *cond) {
10+
lock_init(&cond->core, next_striped_spin_lock_num());
11+
cond->waiter = LOCK_INVALID_OWNER_ID;
12+
cond->broadcast_count = 0;
13+
cond->signaled = false;
14+
__mem_fence_release();
15+
}
16+
17+
bool __time_critical_func(cond_wait_until)(cond_t *cond, mutex_t *mtx, absolute_time_t until) {
18+
bool success = true;
19+
lock_owner_id_t caller = lock_get_caller_owner_id();
20+
uint32_t save = save_and_disable_interrupts();
21+
// Acquire the mutex spin lock
22+
spin_lock_unsafe_blocking(mtx->core.spin_lock);
23+
assert(lock_is_owner_id_valid(mtx->owner));
24+
assert(caller == mtx->owner);
25+
26+
// Mutex and cond spin locks can be the same as spin locks are attributed
27+
// using `next_striped_spin_lock_num()`. To avoid any deadlock, we only
28+
// acquire the condition variable spin lock if it is different from the
29+
// mutex spin lock
30+
bool same_spinlock = mtx->core.spin_lock == cond->core.spin_lock;
31+
32+
// Acquire the condition variable spin_lock
33+
if (!same_spinlock) {
34+
spin_lock_unsafe_blocking(cond->core.spin_lock);
35+
}
36+
37+
// Release the mutex but without restoring interrupts and notify.
38+
mtx->owner = LOCK_INVALID_OWNER_ID;
39+
if (!same_spinlock) {
40+
spin_unlock_unsafe(mtx->core.spin_lock);
41+
}
42+
43+
uint64_t current_broadcast = cond->broadcast_count;
44+
45+
if (lock_is_owner_id_valid(cond->waiter)) {
46+
// There is a valid owner of the condition variable: we are not the
47+
// first waiter.
48+
// First iteration: notify
49+
lock_internal_spin_unlock_with_notify(&cond->core, save);
50+
save = spin_lock_blocking(cond->core.spin_lock);
51+
// Further iterations: wait
52+
do {
53+
if (!lock_is_owner_id_valid(cond->waiter)) {
54+
break;
55+
}
56+
if (cond->broadcast_count != current_broadcast) {
57+
break;
58+
}
59+
if (is_at_the_end_of_time(until)) {
60+
lock_internal_spin_unlock_with_wait(&cond->core, save);
61+
} else {
62+
if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&cond->core, save, until)) {
63+
// timed out
64+
success = false;
65+
break;
66+
}
67+
}
68+
save = spin_lock_blocking(cond->core.spin_lock);
69+
} while (true);
70+
} else {
71+
// Notify to finish release of mutex
72+
__sev();
73+
}
74+
75+
if (success && cond->broadcast_count == current_broadcast) {
76+
cond->waiter = caller;
77+
78+
// Wait for the signal
79+
do {
80+
if (cond->signaled) {
81+
cond->waiter = LOCK_INVALID_OWNER_ID;
82+
cond->signaled = false;
83+
break;
84+
}
85+
if (is_at_the_end_of_time(until)) {
86+
lock_internal_spin_unlock_with_wait(&cond->core, save);
87+
} else {
88+
if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&cond->core, save, until)) {
89+
// timed out
90+
cond->waiter = LOCK_INVALID_OWNER_ID;
91+
success = false;
92+
break;
93+
}
94+
}
95+
save = spin_lock_blocking(cond->core.spin_lock);
96+
} while (true);
97+
}
98+
99+
// We got the signal (or timed out)
100+
// Acquire the mutex spin lock and release the core spin lock.
101+
if (!same_spinlock) {
102+
spin_lock_unsafe_blocking(mtx->core.spin_lock);
103+
spin_unlock_unsafe(cond->core.spin_lock);
104+
}
105+
106+
if (lock_is_owner_id_valid(mtx->owner)) {
107+
// Another core holds the mutex.
108+
// First iteration: notify
109+
lock_internal_spin_unlock_with_notify(&mtx->core, save);
110+
save = spin_lock_blocking(mtx->core.spin_lock);
111+
// Further iterations: wait
112+
do {
113+
if (!lock_is_owner_id_valid(mtx->owner)) {
114+
break;
115+
}
116+
// We always wait for the mutex.
117+
lock_internal_spin_unlock_with_wait(&mtx->core, save);
118+
save = spin_lock_blocking(mtx->core.spin_lock);
119+
} while (true);
120+
} else {
121+
// Notify to finish release of condition variable
122+
__sev();
123+
}
124+
125+
// Eventually hold the mutex.
126+
mtx->owner = caller;
127+
spin_unlock(mtx->core.spin_lock, save);
128+
129+
return success;
130+
}
131+
132+
bool __time_critical_func(cond_wait_timeout_ms)(cond_t *cond, mutex_t *mtx, uint32_t timeout_ms) {
133+
return cond_wait_until(cond, mtx, make_timeout_time_ms(timeout_ms));
134+
}
135+
136+
bool __time_critical_func(cond_wait_timeout_us)(cond_t *cond, mutex_t *mtx, uint32_t timeout_us) {
137+
return cond_wait_until(cond, mtx, make_timeout_time_us(timeout_us));
138+
}
139+
140+
void __time_critical_func(cond_wait)(cond_t *cond, mutex_t *mtx) {
141+
cond_wait_until(cond, mtx, at_the_end_of_time);
142+
}
143+
144+
void __time_critical_func(cond_signal)(cond_t *cond) {
145+
uint32_t save = spin_lock_blocking(cond->core.spin_lock);
146+
if (lock_is_owner_id_valid(cond->waiter)) {
147+
// We have a waiter, we can signal.
148+
cond->signaled = true;
149+
lock_internal_spin_unlock_with_notify(&cond->core, save);
150+
} else {
151+
spin_unlock(cond->core.spin_lock, save);
152+
}
153+
}
154+
155+
void __time_critical_func(cond_broadcast)(cond_t *cond) {
156+
uint32_t save = spin_lock_blocking(cond->core.spin_lock);
157+
if (lock_is_owner_id_valid(cond->waiter)) {
158+
// We have a waiter, we can broadcast.
159+
cond->signaled = true;
160+
cond->broadcast_count++;
161+
lock_internal_spin_unlock_with_notify(&cond->core, save);
162+
} else {
163+
spin_unlock(cond->core.spin_lock, save);
164+
}
165+
}

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

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
/*
2+
* Copyright (c) 2022-2023 Paul Guyot <[email protected]>
3+
*
4+
* SPDX-License-Identifier: BSD-3-Clause
5+
*/
6+
7+
#ifndef _PLATFORM_COND_H
8+
#define _PLATFORM_COND_H
9+
10+
#include "pico/mutex.h"
11+
12+
#ifdef __cplusplus
13+
extern "C" {
14+
#endif
15+
16+
/** \file cond.h
17+
* \defgroup cond cond
18+
* \ingroup pico_sync
19+
* \brief Condition variable API for non IRQ mutual exclusion between cores
20+
*
21+
* Condition variables complement mutexes by providing a way to atomically
22+
* wait and release a held mutex. Then, the task on the other core can signal
23+
* the variable, which ends the wait. Often, the other core would also hold
24+
* the shared mutex, so the signaled task waits until the mutex is released.
25+
*
26+
* Condition variables can also be broadcast.
27+
*
28+
* In this implementation, it is not mandatory. The condition variables only
29+
* work with non-recursive mutexes.
30+
*
31+
* Limitations of mutexes also apply to condition variables. See \ref mutex.h
32+
*/
33+
34+
typedef struct __packed_aligned
35+
{
36+
lock_core_t core;
37+
lock_owner_id_t waiter;
38+
uint32_t broadcast_count; // Overflow is unlikely
39+
bool signaled;
40+
} cond_t;
41+
42+
/*! \brief Initialize a condition variable structure
43+
* \ingroup cond
44+
*
45+
* \param cv Pointer to condition variable structure
46+
*/
47+
void cond_init(cond_t *cv);
48+
49+
/*! \brief Wait on a condition variable
50+
* \ingroup cond
51+
*
52+
* Wait until a condition variable is signaled or broadcast. The mutex should
53+
* be owned and is released atomically. It is reacquired when this function
54+
* returns.
55+
*
56+
* \param cv Condition variable to wait on
57+
* \param mtx Currently held mutex
58+
*/
59+
void cond_wait(cond_t *cv, mutex_t *mtx);
60+
61+
/*! \brief Wait on a condition variable with a timeout.
62+
* \ingroup cond
63+
*
64+
* Wait until a condition variable is signaled or broadcast until a given
65+
* time. The mutex is released atomically and reacquired even if the wait
66+
* timed out.
67+
*
68+
* \param cv Condition variable to wait on
69+
* \param mtx Currently held mutex
70+
* \param until The time after which to return if the condition variable was
71+
* not signaled.
72+
* \return true if the condition variable was signaled, false otherwise
73+
*/
74+
bool cond_wait_until(cond_t *cv, mutex_t *mtx, absolute_time_t until);
75+
76+
/*! \brief Wait on a condition variable with a timeout.
77+
* \ingroup cond
78+
*
79+
* Wait until a condition variable is signaled or broadcast until a given
80+
* time. The mutex is released atomically and reacquired even if the wait
81+
* timed out.
82+
*
83+
* \param cv Condition variable to wait on
84+
* \param mtx Currently held mutex
85+
* \param timeout_ms The timeout in milliseconds.
86+
* \return true if the condition variable was signaled, false otherwise
87+
*/
88+
bool cond_wait_timeout_ms(cond_t *cv, mutex_t *mtx, uint32_t timeout_ms);
89+
90+
/*! \brief Wait on a condition variable with a timeout.
91+
* \ingroup cond
92+
*
93+
* Wait until a condition variable is signaled or broadcast until a given
94+
* time. The mutex is released atomically and reacquired even if the wait
95+
* timed out.
96+
*
97+
* \param cv Condition variable to wait on
98+
* \param mtx Currently held mutex
99+
* \param timeout_ms The timeout in microseconds.
100+
* \return true if the condition variable was signaled, false otherwise
101+
*/
102+
bool cond_wait_timeout_us(cond_t *cv, mutex_t *mtx, uint32_t timeout_us);
103+
104+
/*! \brief Signal on a condition variable and wake the waiter
105+
* \ingroup cond
106+
*
107+
* \param cv Condition variable to signal
108+
*/
109+
void cond_signal(cond_t *cv);
110+
111+
/*! \brief Broadcast a condition variable and wake every waiters
112+
* \ingroup cond
113+
*
114+
* \param cv Condition variable to signal
115+
*/
116+
void cond_broadcast(cond_t *cv);
117+
118+
#ifdef __cplusplus
119+
}
120+
#endif
121+
#endif

‎src/common/pico_sync/include/pico/sync.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,5 +15,6 @@
1515
#include "pico/sem.h"
1616
#include "pico/mutex.h"
1717
#include "pico/critical_section.h"
18+
#include "pico/cond.h"
1819

1920
#endif

‎test/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,4 +11,5 @@ if (PICO_ON_DEVICE)
1111
add_subdirectory(hardware_pwm_test)
1212
add_subdirectory(cmsis_test)
1313
add_subdirectory(pico_sem_test)
14+
add_subdirectory(pico_cond_test)
1415
endif()

‎test/pico_cond_test/CMakeLists.txt‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
add_executable(pico_cond_test pico_cond_test.c)
2+
3+
target_link_libraries(pico_cond_test PRIVATE pico_test pico_sync pico_multicore pico_stdlib )
4+
pico_add_extra_outputs(pico_cond_test)
5+
6+
target_compile_definitions(pico_cond_test PRIVATE
7+
PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS=-1) # wait for USB connect
8+
9+
pico_add_extra_outputs(pico_cond_test)
10+
pico_enable_stdio_uart(pico_cond_test 0)
11+
pico_enable_stdio_usb(pico_cond_test 1)

0 commit comments

Comments
 (0)