fix(simu): simulator shutdown (#6196)

Co-authored-by: philmoz <phil.a.mitchell@gmail.com>

radio/src/os/sleep_native.cpp

@@ -20,18 +20,7 @@
  */
 
 #include "sleep.h"
+#include "task.h"
 
-#include <thread>
-
-extern uint8_t simuSleep(uint32_t ms);
-
-void sleep_ms(uint32_t ms)
-{
-  simuSleep(ms);
-}
-
-void sleep_until(time_point_t* tp, uint32_t inc)
-{
-  *tp += std::chrono::duration<uint32_t, std::milli>{inc};
-  std::this_thread::sleep_until(*tp);
-}
+void sleep_ms(uint32_t ms) { task_sleep_ms(ms); }
+void sleep_until(time_point_t* tp, uint32_t inc) { task_sleep_until(tp, inc); }

radio/src/os/task_freertos.h

@@ -39,3 +39,5 @@ struct mutex_handle_t {
   SemaphoreHandle_t _rtos_handle;
   StaticSemaphore_t _mutex_struct;
 };
+
+static inline bool task_running() { return true; }

radio/src/os/task_pthread.cpp

@@ -20,19 +20,101 @@
  */
 
 #include "task.h"
+#include "debug.h"
+#include "timer_pthread_impl.h"
+
+#include <pthread.h>
+#include <chrono>
+#include <vector>
+#include <mutex>
+
+static std::condition_variable _stop_cv;
+static std::mutex _stop_m;
+
+static std::mutex _tasks_m;
+static std::vector<pthread_t> _tasks;
+static bool _stop_tasks = false;
+
+void task_sleep_ms(uint32_t ms)
+{
+  std::unique_lock<std::mutex> lk(_stop_m);
+  _stop_cv.wait_for(lk, std::chrono::milliseconds(ms));
+}
+
+void task_sleep_until(time_point_t* tp, uint32_t inc)
+{
+  *tp += std::chrono::duration<uint32_t, std::milli>{inc};
+
+  std::unique_lock<std::mutex> lk(_stop_m);
+  _stop_cv.wait_until(lk, *tp);
+}
+
+static void stop_tasks()
+{
+  std::lock_guard lock(_tasks_m);
+  _stop_tasks = true;
+}
+
+static bool next_task_to_stop(pthread_t& task)
+{
+  std::lock_guard lock(_tasks_m);
+
+  // no more tasks
+  if (_tasks.empty()) return false;
+
+  // grab the next one
+  task = _tasks.back();
+  _tasks.pop_back();
+
+  return true;
+}
+
+void task_shutdown_all()
+{
+  stop_tasks();
+  _stop_cv.notify_all();
+
+  pthread_t task;
+  while (next_task_to_stop(task)) {
+    pthread_join(task, nullptr);
+  }
+
+  timer_queue::instance().stop();
+}
+
+struct run_context {
+  task_func_t func;
+  std::string name;
+};
 
 static void* _task_stub(void* p)
 {
-  task_func_t func = (task_func_t)p;
-  func();
+  std::unique_ptr<run_context> ctx{(run_context*)p};
+  auto name = ctx->name.c_str();
+
+  TRACE("<%s> started", name);
+  ctx->func();
+  TRACE("<%s> stopped", name);
+
   return nullptr;
 }
 
 void task_create(task_handle_t* h, task_func_t func, const char* name,
                  void* stack, unsigned stack_size, unsigned priority)
 {
+  std::lock_guard lock(_tasks_m);
+  if (_stop_tasks) return;
+
   h->_stack_size = stack_size;
-  pthread_create(&h->_thread_handle, nullptr, _task_stub, (void*)func);
+  run_context* ctx = new run_context{func, name};
+  if (pthread_create(&h->_thread_handle, nullptr, _task_stub, (void*)ctx) == 0) {
+    _tasks.emplace_back(h->_thread_handle);
+  }
+}
+
+bool task_running()
+{
+  return !_stop_tasks;
 }
 
 unsigned task_get_stack_usage(task_handle_t* h)
@@ -48,7 +130,7 @@ unsigned task_get_stack_size(task_handle_t* h)
 
 void mutex_create(mutex_handle_t* h)
 {
-  *h = PTHREAD_MUTEX_INITIALIZER;
+  pthread_mutex_init(h, nullptr);
 }
 
 bool mutex_lock(mutex_handle_t* h)

radio/src/os/task_pthread.h

@@ -21,11 +21,11 @@
 
 #pragma once
 
+#include "time.h"
+
 #include <pthread.h>
 #include <semaphore.h>
 
-#include "definitions.h"
-
 #define TASK_DEFINE_STACK(name, size) void* name
 
 struct task_handle_t {
@@ -34,3 +34,10 @@ struct task_handle_t {
 };
 
 typedef pthread_mutex_t mutex_handle_t;
+
+bool task_running();
+
+void task_sleep_ms(uint32_t ms);
+void task_sleep_until(time_point_t* tp, uint32_t ts_ms);
+
+void task_shutdown_all();

radio/src/os/timer_pthread.cpp

@@ -1,4 +1,5 @@
 #include "timer.h"
+#include "debug.h"
 #include "timer_pthread_impl.h"
 
 #include <algorithm>
@@ -7,7 +8,9 @@ bool _timer_cmp(timer_handle_t *lh, timer_handle_t *rh) {
   return lh->next_trigger < rh->next_trigger;
 }
 
-timer_queue::timer_queue() {}
+timer_queue::timer_queue() {
+  start();
+}
 
 void timer_queue::update_current_time() {
   _current_time = std::chrono::steady_clock::now();
@@ -47,6 +50,7 @@ void timer_queue::stop() {
     slock.unlock();
   }
   _thread->join();
+  TRACE("<timer_queue> stopped");
 }
 
 void timer_queue::create_timer(timer_handle_t *timer, timer_func_t func, const char *name,
@@ -84,6 +88,7 @@ void timer_queue::pend_function(timer_async_func_t func, void *param1,
 
 void timer_queue::main_loop() {
 
+  TRACE("<timer_queue> started");
   while (true) {
     {
       std::unique_lock lock(_cmds_mutex);