Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

rp2040 baremetal threading #291

Closed
wants to merge 9 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 0 additions & 8 deletions core/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
292 changes: 286 additions & 6 deletions core/platform/lf_rp2040_support.c
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,10 @@ THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <pico/stdlib.h>
#include <pico/multicore.h>
#include <pico/util/queue.h>
#include <pico/sync.h>

// unthreaded statics
/**
* critical section struct
* disables external irq and core execution
Expand All @@ -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
}

/**
Expand All @@ -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;
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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

8 changes: 8 additions & 0 deletions include/core/platform.h
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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__)
Expand Down
Loading
Loading