multi-thread in the Patmos Platform

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 <pthread.h>
+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

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;
 }

low_level_platform/impl/src/lf_patmos_support.c

@@ -15,11 +15,16 @@
 #include <machine/rtc.h>
 #include <machine/exceptions.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <pthread.h>
+
+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

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;