diff --git a/low_level_platform/api/platform/lf_patmos_support.h b/low_level_platform/api/platform/lf_patmos_support.h index ccc4e46d0..33ba4492c 100644 --- a/low_level_platform/api/platform/lf_patmos_support.h +++ b/low_level_platform/api/platform/lf_patmos_support.h @@ -22,5 +22,22 @@ #define PRINTF_TIME "%" PRId64 #define PRINTF_MICROSTEP "%" PRIu32 #define PRINTF_TAG "(%" PRId64 ", %" PRIu32 ")" +#if !defined(LF_SINGLE_THREADED) +#include +typedef struct { + pthread_t handle; + int cpuid; +} lf_thread_t; +typedef pthread_mutex_t lf_mutex_t; +typedef struct { + lf_mutex_t* mutex; + pthread_cond_t condition; +} lf_cond_t; +#if !defined(LF_SINGLE_THREADED) +// Cross-core global lock used by atomic implementations on Patmos. +void _lf_patmos_global_lock_acquire(void); +void _lf_patmos_global_lock_release(void); +#endif +#endif #endif // LF_PATMOS_SUPPORT_H diff --git a/low_level_platform/impl/src/lf_atomic_irq.c b/low_level_platform/impl/src/lf_atomic_irq.c index 5f1346743..520662681 100644 --- a/low_level_platform/impl/src/lf_atomic_irq.c +++ b/low_level_platform/impl/src/lf_atomic_irq.c @@ -20,74 +20,122 @@ int lf_enable_interrupts_nested(); int lf_atomic_fetch_add(int* ptr, int value) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif int res = *ptr; *ptr += value; +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } int64_t lf_atomic_fetch_add64(int64_t* ptr, int64_t value) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif int64_t res = *ptr; *ptr += value; +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } int lf_atomic_add_fetch(int* ptr, int value) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif int res = *ptr + value; *ptr = res; +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } int64_t lf_atomic_add_fetch64(int64_t* ptr, int64_t value) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif int64_t res = *ptr + value; *ptr = res; +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } bool lf_atomic_bool_compare_and_swap(int* ptr, int oldval, int newval) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif bool res = false; if ((*ptr) == oldval) { *ptr = newval; res = true; } +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } bool lf_atomic_bool_compare_and_swap64(int64_t* ptr, int64_t oldval, int64_t newval) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif bool res = false; if ((*ptr) == oldval) { *ptr = newval; res = true; } +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } int lf_atomic_val_compare_and_swap(int* ptr, int oldval, int newval) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif int res = *ptr; if ((*ptr) == oldval) { *ptr = newval; } +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } int64_t lf_atomic_val_compare_and_swap64(int64_t* ptr, int64_t oldval, int64_t newval) { lf_disable_interrupts_nested(); +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_acquire(); +#endif int64_t res = *ptr; if ((*ptr) == oldval) { *ptr = newval; } +#if defined(PLATFORM_PATMOS) && !defined(LF_SINGLE_THREADED) + _lf_patmos_global_lock_release(); +#endif lf_enable_interrupts_nested(); return res; } diff --git a/low_level_platform/impl/src/lf_patmos_support.c b/low_level_platform/impl/src/lf_patmos_support.c index 4c3fc0fab..ffd6506e4 100644 --- a/low_level_platform/impl/src/lf_patmos_support.c +++ b/low_level_platform/impl/src/lf_patmos_support.c @@ -15,11 +15,16 @@ #include #include #include +#include +#include + +int lf_disable_interrupts_nested(void); +int lf_enable_interrupts_nested(void); // Keep track of physical actions being entered into the system static volatile bool _lf_async_event = false; // Keep track of whether we are in a critical section or not -static volatile int _lf_num_nested_critical_sections = 0; + /** * @brief Sleep until an absolute time. * Since there is no sleep mode in Patmos, and energy saving is not important for real-time systems, @@ -90,9 +95,14 @@ int _lf_clock_gettime(instant_t* t) { #if defined(LF_SINGLE_THREADED) +static volatile int _lf_num_nested_critical_sections = 0; + int lf_disable_interrupts_nested() { + // For the single-threaded path, disable interrupts first then increment + // the nesting counter to avoid preemption windows on this core. + intr_disable(); if (_lf_num_nested_critical_sections++ == 0) { - intr_disable(); + // already disabled above } return 0; } @@ -112,6 +122,188 @@ int _lf_single_threaded_notify_of_event() { _lf_async_event = true; return 0; } -#endif // LF_SINGLE_THREADED +#else // multi threaded Patmos implementation + +/* Dynamically allocate per-core nesting counters based on get_cpucnt(). + * This avoids a hard-coded upper bound and prevents accidental aliasing + * of core IDs to index 0 if the platform reports more cores than the + * compile-time constant. Allocation happens once during validation. */ +static volatile int* _lf_num_nested_critical_sections_by_core = NULL; +static int _lf_patmos_max_cores = 0; +static pthread_mutex_t _lf_patmos_core_configuration_mutex = PTHREAD_MUTEX_INITIALIZER; + +static void _lf_initialize_patmos_core_configuration(void) { + int cpucnt = (int)get_cpucnt(); + assert(cpucnt > 0); + /* Allocate the per-core nesting counter array once and publish it atomically + * via pthread_once so all cores observe the same initialized state. */ + _lf_num_nested_critical_sections_by_core = (volatile int*)calloc((size_t)cpucnt, sizeof(int)); + assert(_lf_num_nested_critical_sections_by_core != NULL); + _lf_patmos_max_cores = cpucnt; +} + +static inline void _lf_validate_patmos_core_configuration(void) { + int result = pthread_mutex_lock(&_lf_patmos_core_configuration_mutex); + assert(result == 0); + if (_lf_num_nested_critical_sections_by_core == NULL) { + _lf_initialize_patmos_core_configuration(); + } else { + assert((int)get_cpucnt() == _lf_patmos_max_cores); + } + result = pthread_mutex_unlock(&_lf_patmos_core_configuration_mutex); + assert(result == 0); +} + +static inline volatile int* _lf_current_core_nested_counter() { + int cpucnt; + int cpuid; + + _lf_validate_patmos_core_configuration(); + cpucnt = (int)get_cpucnt(); + cpuid = (int)get_cpuid(); + assert(cpuid >= 0); + assert(cpuid < cpucnt); + return &_lf_num_nested_critical_sections_by_core[cpuid]; +} + +// Global (cross-core) lock for atomic operations. + +static pthread_mutex_t _lf_patmos_global_lock = PTHREAD_MUTEX_INITIALIZER; + +void _lf_patmos_global_lock_acquire(void) { pthread_mutex_lock(&_lf_patmos_global_lock); } +void _lf_patmos_global_lock_release(void) { pthread_mutex_unlock(&_lf_patmos_global_lock); } + + +int lf_disable_interrupts_nested() { + // Disable interrupts first and increment the per-core nesting counter. + intr_disable(); + volatile int* nested_counter = _lf_current_core_nested_counter(); + (*nested_counter)++; + return 0; +} + +int lf_enable_interrupts_nested() { + volatile int* nested_counter = _lf_current_core_nested_counter(); + if (*nested_counter <= 0) { + return 1; + } + + if (--(*nested_counter) == 0) { + intr_enable(); + } + return 0; +} + +int lf_available_cores() { return (int)get_cpucnt(); } + +lf_thread_t lf_thread_self() { + lf_thread_t self = {0}; + self.cpuid = (int)get_cpuid(); + return self; +} + +int lf_thread_create(lf_thread_t* thread, void* (*lf_thread)(void*), void* arguments) { + assert(thread != NULL); + return pthread_create(&thread->handle, NULL, lf_thread, arguments); +} + +int lf_thread_join(lf_thread_t thread, void** thread_return) { + return pthread_join(thread.handle, thread_return); +} + +int lf_thread_id() { return (int)get_cpuid(); } + +/* Forward declaration for the runtime core-count check. */ +void initialize_lf_thread_id_check(void); + +void initialize_lf_thread_id() { initialize_lf_thread_id_check(); } + +// Validate platform assumptions at initialization time. +void initialize_lf_thread_id_check() { + // Ensure per-core counters are allocated and runtime core count remains stable. + _lf_validate_patmos_core_configuration(); + assert((int)get_cpucnt() == _lf_patmos_max_cores); +} + +int lf_mutex_init(lf_mutex_t* mutex) { + int result; + pthread_mutexattr_t attr; + + assert(mutex != NULL); + + result = pthread_mutexattr_init(&attr); + if (result != 0) { + return result; + } + + result = pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + if (result != 0) { + pthread_mutexattr_destroy(&attr); + return result; + } + + result = pthread_mutex_init((pthread_mutex_t*)mutex, &attr); + pthread_mutexattr_destroy(&attr); + return result; +} + +int lf_mutex_lock(lf_mutex_t* mutex) { + assert(mutex != NULL); + return pthread_mutex_lock((pthread_mutex_t*)mutex); +} + +int lf_mutex_unlock(lf_mutex_t* mutex) { + assert(mutex != NULL); + return pthread_mutex_unlock((pthread_mutex_t*)mutex); +} + +int lf_cond_init(lf_cond_t* cond, lf_mutex_t* mutex) { + assert(cond != NULL); + assert(mutex != NULL); + cond->mutex = mutex; + return pthread_cond_init((pthread_cond_t*)&cond->condition, NULL); +} + +int lf_cond_signal(lf_cond_t* cond) { + assert(cond != NULL); + return pthread_cond_signal((pthread_cond_t*)&cond->condition); +} + +int lf_cond_broadcast(lf_cond_t* cond) { + assert(cond != NULL); + return pthread_cond_broadcast((pthread_cond_t*)&cond->condition); +} + +int lf_cond_wait(lf_cond_t* cond) { + assert(cond != NULL); + assert(cond->mutex != NULL); + return pthread_cond_wait((pthread_cond_t*)&cond->condition, (pthread_mutex_t*)cond->mutex); +} + +int _lf_cond_timedwait(lf_cond_t* cond, instant_t wakeup_time) { + struct timespec ts; + int rc; + + assert(cond != NULL); + assert(cond->mutex != NULL); + + instant_t now; + _lf_clock_gettime(&now); + + if (now >= wakeup_time) { + return LF_TIMEOUT; + } + + ts.tv_sec = wakeup_time / 1000000000LL; + ts.tv_nsec = wakeup_time % 1000000000LL; + + rc = pthread_cond_timedwait((pthread_cond_t*)&cond->condition, (pthread_mutex_t*)cond->mutex, &ts); + if (rc == ETIMEDOUT) { + return LF_TIMEOUT; + } + return rc; +} + +#endif #endif // PLATFORM_PATMOS diff --git a/low_level_platform/impl/src/lf_platform_util.c b/low_level_platform/impl/src/lf_platform_util.c index 212e6ea83..398f3335b 100644 --- a/low_level_platform/impl/src/lf_platform_util.c +++ b/low_level_platform/impl/src/lf_platform_util.c @@ -13,7 +13,7 @@ int map_priorities(int priority, int dest_min, int dest_max) { (LF_SCHED_MAX_PRIORITY - LF_SCHED_MIN_PRIORITY)); } -#ifndef PLATFORM_ZEPHYR // on Zephyr, this is handled separately +#if !defined(PLATFORM_ZEPHYR) && !defined(PLATFORM_PATMOS) // on Zephyr and PATMOS, this is handled separately #ifndef LF_SINGLE_THREADED static int _lf_worker_thread_count = 0;