diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a9026392c..469a40bf8 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -74,14 +74,6 @@ if(DEFINED _LF_CLOCK_SYNC_ON) endif() endif() -# Link with thread library, unless if we are targeting the Zephyr RTOS -if(DEFINED LF_THREADED OR DEFINED LF_TRACE) - if(NOT ${CMAKE_SYSTEM_NAME} STREQUAL "Zephyr") - find_package(Threads REQUIRED) - target_link_libraries(core PUBLIC Threads::Threads) - endif() -endif() - # Macro for translating a command-line argument into compile definition for # core lib macro(define X) diff --git a/core/platform/lf_rp2040_support.c b/core/platform/lf_rp2040_support.c index 8cbc0985e..e76d179ba 100644 --- a/core/platform/lf_rp2040_support.c +++ b/core/platform/lf_rp2040_support.c @@ -38,8 +38,10 @@ THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include +// unthreaded statics /** * critical section struct * disables external irq and core execution @@ -57,16 +59,33 @@ static semaphore_t _lf_sem_irq_event; // nested critical section counter static uint32_t _lf_num_nested_crit_sec = 0; +// threaded statics +/** + * binary semaphore used to synchronize + * used by thread join + */ +static mutex_t _lf_core_sync; +/** + * track number of threads created + * error on value greater than 2 + */ +static uint8_t _lf_thread_cnt = 0; + /** * Initialize basic runtime infrastructure and * synchronization structs for an unthreaded runtime. */ void _lf_initialize_clock(void) { // init stdio lib + // for debug printf stdio_init_all(); - // init sync structs + critical_section_init(&_lf_crit_sec); sem_init(&_lf_sem_irq_event, 0, 1); + // only init sync mutex in multicore +#ifdef LF_THREADED + mutex_init(&_lf_core_sync); +#endif //LF_THREADED } /** @@ -83,7 +102,7 @@ int _lf_clock_now(instant_t* t) { } // time struct absolute_time_t now; - uint64_t ns_from_boot; + int64_t ns_from_boot; now = get_absolute_time(); ns_from_boot = to_us_since_boot(now) * 1000; @@ -145,7 +164,6 @@ int _lf_interruptable_sleep_until_locked(environment_t* env, instant_t wakeup_ti return ret_code; } -#ifdef LF_UNTHREADED /** * The single thread RP2040 platform support treats second core * routines similar to external interrupt routine threads. @@ -210,11 +228,273 @@ int _lf_unthreaded_notify_of_event() { sem_release(&_lf_sem_irq_event); return 0; } -#endif //LF_UNTHREADED #ifdef LF_THREADED -#error "Threading for baremetal RP2040 not supported" -#endif //LF_THREADED +// FIXME: add validator check that invalidates threaded rp2040 +// lf programs with more than 2 workers set +#undef NUMBER_OF_WORKERS +#define NUMBER_OF_WORKERS 2 + +static lf_function_t _lf_core0_worker, _lf_core1_worker; +static void *_lf_core0_args, *_lf_core1_args; + + +void _rp2040_core1_entry() { + // lock sync lock + mutex_enter_blocking(&_lf_core_sync); + void *res = _lf_core1_worker(_lf_core1_args); + // unlock sync lock + mutex_exit(&_lf_core_sync); +} + +/** + * @brief Get the number of cores on the host machine. + */ +int lf_available_cores() { + return 2; +} + +/** + * Create a new thread, starting with execution of lf_thread + * getting passed arguments. The new handle is stored in thread_id. + * + * @return 0 on success, platform-specific error number otherwise. + * + */ +int lf_thread_create(lf_thread_t* thread, void *(*lf_thread) (void *), void* arguments) { + if (_lf_thread_cnt == 0) { + *thread = RP2040_CORE_0; + _lf_core0_worker = (lf_function_t) lf_thread; + _lf_core0_args = arguments; + } else if (_lf_thread_cnt == 1) { + *thread = RP2040_CORE_1; + _lf_core1_worker = (lf_function_t) lf_thread; + _lf_core1_args = arguments; + multicore_launch_core1(_rp2040_core1_entry); + } else { + // invalid thread + LF_PRINT_DEBUG("rp2040: invalid thread create id"); + return -1; + } + _lf_thread_cnt++; + return 0; +} + +/** + * Make calling thread wait for termination of the thread. The + * exit status of the thread is stored in thread_return if thread_return + * is not NULL. + * @param thread The thread. + * @param thread_return A pointer to where to store the exit status of the thread. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_thread_join(lf_thread_t thread, void** thread_return) { + switch(thread) { + case RP2040_CORE_0: + // run core0 worker + // block until completion + *thread_return = _lf_core0_worker(_lf_core0_args); + break; + case RP2040_CORE_1: + // block until core sync released + mutex_enter_blocking(&_lf_core_sync); + break; + default: + LF_PRINT_DEBUG("rp2040: invalid thread join id"); + return -1; + } + return 0; +} + +/** + * Initialize a mutex. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_mutex_init(lf_mutex_t* mutex) { + recursive_mutex_init(mutex); + return 0; +} + +/** + * Lock a mutex. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_mutex_lock(lf_mutex_t* mutex) { + recursive_mutex_enter_blocking(mutex); + return 0; +} + +/** + * Unlock a mutex. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_mutex_unlock(lf_mutex_t* mutex) { + recursive_mutex_exit(mutex); + return 0; +} + + +// FIXME: bugged since cond variables +// have different behavior compared to a sempahore + +/** + * Initialize a conditional variable. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_cond_init(lf_cond_t* cond, lf_mutex_t* mutex) { + // reference to mutex + cond->mutex = mutex; + // init queue, use core num as debug info + // max number of entries in the queue is equal to number of cores + queue_init(&cond->signal, sizeof(uint32_t), 2); + return 0; +} + +/** + * Wake up all threads waiting for condition variable cond. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_cond_broadcast(lf_cond_t* cond) { + // notify both cores + // add to queue, non blocking + // this method could be called from an isr + uint32_t core = get_core_num(); + queue_try_add(&cond->signal, &core); + return queue_try_add(&cond->signal, &core) ? 0 : -1; +} + +/** + * Wake up one thread waiting for condition variable cond. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_cond_signal(lf_cond_t* cond) { + // release permit + // add to queue, non blocking + uint32_t core = get_core_num(); + return queue_try_add(&cond->signal, &core) ? 0 : -1; +} +/** + * Wait for condition variable "cond" to be signaled or broadcast. + * "mutex" is assumed to be locked before. + * + * @return 0 on success, platform-specific error number otherwise. + */ +int lf_cond_wait(lf_cond_t* cond) { + uint32_t cur_core, queue_core; + cur_core = get_core_num(); + // unlock mutex + lf_mutex_unlock(cond->mutex); + // queue remove blocking + queue_remove_blocking(&cond->signal, &queue_core); + // debug: check calling core + if (cur_core == queue_core) { + LF_PRINT_DEBUG("rp2040: self core cond release"); + } + lf_mutex_lock(cond->mutex); + return 0; +} + +/** + * Block current thread on the condition variable until condition variable + * pointed by "cond" is signaled or time pointed by "absolute_time_ns" in + * nanoseconds is reached. + * + * @return 0 on success, LF_TIMEOUT on timeout, and platform-specific error + * number otherwise. + */ +int lf_cond_timedwait(lf_cond_t* cond, instant_t absolute_time_ns) { + uint32_t cur_core, queue_core; + cur_core = get_core_num(); + // unlock mutex + lf_mutex_unlock(cond->mutex); + absolute_time_t target, now; + // check timeout + if (absolute_time_ns < 0) { + return LF_TIMEOUT; + } + // target and cur time + target = from_us_since_boot((uint64_t) (absolute_time_ns / 1000)); + now = get_absolute_time(); + while (!queue_try_remove(&cond->signal, &queue_core)) { + // todo: cases where this might overflow + if (absolute_time_diff_us(now, target) < 0) { + lf_mutex_lock(cond->mutex); + return LF_TIMEOUT; + } + now = get_absolute_time(); + } + if (cur_core == queue_core) { + LF_PRINT_DEBUG("rp2040: self core cond release"); + } + lf_mutex_lock(cond->mutex); + return 0; +} + +/** + * Atomics for the rp2040 platform. + * note: uses the same implementation as the zephyr platform + * TODO: explore more efficent options + */ +/** + * @brief Add `value` to `*ptr` and return original value of `*ptr` + */ +int _rp2040_atomic_fetch_add(int *ptr, int value) { + lf_disable_interrupts_nested(); + int res = *ptr; + *ptr += value; + lf_enable_interrupts_nested(); + return res; +} +/** + * @brief Add `value` to `*ptr` and return new updated value of `*ptr` + */ +int _rp2040_atomic_add_fetch(int *ptr, int value) { + lf_disable_interrupts_nested(); + int res = *ptr + value; + *ptr = res; + lf_enable_interrupts_nested(); + return res; +} + +/** + * @brief Compare and swap for boolaen value. + * If `*ptr` is equal to `value` then overwrite it + * with `newval`. If not do nothing. Retruns true on overwrite. + */ +bool _rp2040_bool_compare_and_swap(bool *ptr, bool value, bool newval) { + lf_disable_interrupts_nested(); + bool res = false; + if (*ptr == value) { + *ptr = newval; + res = true; + } + lf_enable_interrupts_nested(); + return res; +} + +/** + * @brief Compare and swap for integers. If `*ptr` is equal + * to `value`, it is updated to `newval`. The function returns + * the original value of `*ptr`. + */ +int _rp2040_val_compare_and_swap(int *ptr, int value, int newval) { + lf_disable_interrupts_nested(); + int res = *ptr; + if (*ptr == value) { + *ptr = newval; + } + lf_enable_interrupts_nested(); + return res; +} + +#endif //LF_THREADED #endif // PLATFORM_RP2040 diff --git a/include/core/platform.h b/include/core/platform.h index a32cfb516..5505e2d02 100644 --- a/include/core/platform.h +++ b/include/core/platform.h @@ -228,6 +228,8 @@ int lf_cond_timedwait(lf_cond_t* cond, instant_t absolute_time_ns); */ #if defined(PLATFORM_ZEPHYR) #define lf_atomic_fetch_add(ptr, value) _zephyr_atomic_fetch_add((int*) ptr, value) +#elif defined(PLATFORM_RP2040) +#define lf_atomic_fetch_add(ptr, value) _rp2040_atomic_fetch_add((int*) ptr, value) #elif defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) // Assume that an integer is 32 bits. #define lf_atomic_fetch_add(ptr, value) InterlockedExchangeAdd(ptr, value) @@ -245,6 +247,8 @@ int lf_cond_timedwait(lf_cond_t* cond, instant_t absolute_time_ns); */ #if defined(PLATFORM_ZEPHYR) #define lf_atomic_add_fetch(ptr, value) _zephyr_atomic_add_fetch((int*) ptr, value) +#elif defined(PLATFORM_RP2040) +#define lf_atomic_add_fetch(ptr, value) _rp2040_atomic_add_fetch((int*) ptr, value) #elif defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) // Assume that an integer is 32 bits. #define lf_atomic_add_fetch(ptr, value) InterlockedAdd(ptr, value) @@ -264,6 +268,8 @@ int lf_cond_timedwait(lf_cond_t* cond, instant_t absolute_time_ns); */ #if defined(PLATFORM_ZEPHYR) #define lf_bool_compare_and_swap(ptr, value, newval) _zephyr_bool_compare_and_swap((bool*) ptr, value, newval) +#elif defined(PLATFORM_RP2040) +#define lf_bool_compare_and_swap(ptr, value, newval) _rp2040_bool_compare_and_swap((bool*) ptr, value, newval) #elif defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) // Assume that a boolean is represented with a 32-bit integer. #define lf_bool_compare_and_swap(ptr, oldval, newval) (InterlockedCompareExchange(ptr, newval, oldval) == oldval) @@ -283,6 +289,8 @@ int lf_cond_timedwait(lf_cond_t* cond, instant_t absolute_time_ns); */ #if defined(PLATFORM_ZEPHYR) #define lf_val_compare_and_swap(ptr, value, newval) _zephyr_val_compare_and_swap((int*) ptr, value, newval) +#elif defined(PLATFORM_RP2040) +#define lf_val_compare_and_swap(ptr, value, newval) _rp2040_val_compare_and_swap((int*) ptr, value, newval) #elif defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) #define lf_val_compare_and_swap(ptr, oldval, newval) InterlockedCompareExchange(ptr, newval, oldval) #elif defined(__GNUC__) || defined(__clang__) diff --git a/include/core/platform/lf_rp2040_support.h b/include/core/platform/lf_rp2040_support.h index b522d9e65..b1a5170e4 100644 --- a/include/core/platform/lf_rp2040_support.h +++ b/include/core/platform/lf_rp2040_support.h @@ -9,6 +9,7 @@ #include #include +#include #define NO_TTY @@ -21,7 +22,38 @@ #define _LF_TIMEOUT 1 #ifdef LF_THREADED -#error "Threading for baremetal RP2040 not supported" -#endif // LF_THREADED +// function ptr typedef +// @param void ptr +// @return void ptr +typedef void *(*lf_function_t) (void *); + +// mutex primitive +// backed by hardware spinlock +typedef recursive_mutex_t lf_mutex_t; + +// condition var primitive +typedef struct { + lf_mutex_t* mutex; + queue_t signal; +} lf_cond_t; + +// thread id type +// use enum due to limited number of workers +typedef enum { + RP2040_CORE_0 = 0, + RP2040_CORE_1 = 1, +} lf_thread_t; + +// core1 entry method +void _rp2040_core1_entry(); + +// atomics +int _rp2040_atomic_fetch_add(int *ptr, int value); +int _rp2040_atomic_add_fetch(int *ptr, int value); +bool _rp2040_bool_compare_and_swap(bool *ptr, bool value, bool newval); +int _rp2040_val_compare_and_swap(int *ptr, int value, int newval); + + +#endif // LF_THREADED #endif // LF_PICO_SUPPORT_H