fix(simu): simulator shutdown (#6196)

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

radio/src/mixer_scheduler.cpp

@@ -22,6 +22,7 @@
 #include "mixer_scheduler.h"
 #include "tasks/mixer_task.h"
 #include "hal/usb_driver.h"
+#include "os/sleep.h"
 
 #include "dataconstants.h"
 #include <string.h>
@@ -47,7 +48,7 @@ bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
     return true;
   }
 #else
-  simuSleep(timeoutMs);
+  sleep_ms(timeoutMs);
   return true;
 #endif
 }

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

radio/src/targets/simu/simpgmspace.cpp

@@ -34,6 +34,7 @@
 #include "hal/usb_driver.h"
 #include "hal/audio_driver.h"
 
+#include "os/sleep.h"
 #include "os/task.h"
 #include "os/timer_pthread_impl.h"
 
@@ -214,7 +215,6 @@ void simuStart(bool tests, const char * sdPath, const char * settingsPath)
   lcdInit();
 
 #if !defined(SIMU_BOOTLOADER)
-  timer_queue::instance().start();
   simuMain();
 #else
   pthread_attr_t attr;
@@ -238,6 +238,9 @@ void simuStart(bool tests, const char * sdPath, const char * settingsPath)
 
 extern task_handle_t mixerTaskId;
 extern task_handle_t menusTaskId;
+#if defined(AUDIO)
+extern task_handle_t audioTaskId;
+#endif
 
 void simuStop()
 {
@@ -245,9 +248,7 @@ void simuStop()
     return;
 
   simu_shutdown = true;
-
-  pthread_join(mixerTaskId._thread_handle, nullptr);
-  pthread_join(menusTaskId._thread_handle, nullptr);
+  task_shutdown_all();
 
 #if defined(SIMU_AUDIO)
   stopAudio();
@@ -268,15 +269,6 @@ bool simuIsRunning()
   return simu_running;
 }
 
-uint8_t simuSleep(uint32_t ms)
-{
-  for (uint32_t i = 0; i < ms; i++) {
-    if (simu_shutdown || !simu_running) return 1;
-    usleep(1);
-  }
-  return 0;
-}
-
 void audioConsumeCurrentBuffer()
 {
 }
@@ -468,7 +460,7 @@ void delay_us(uint16_t us) { }
 
 void flashWrite(uint32_t *address, const uint32_t *buffer)
 {
-  simuSleep(10);
+  sleep_ms(10);
 }
 
 uint32_t isBootloaderStart(const uint8_t * block)

radio/src/targets/simu/simpgmspace.h

@@ -36,7 +36,6 @@
 extern char * main_thread_error;
 
 uint64_t simuTimerMicros(void);
-uint8_t simuSleep(uint32_t ms);  // returns true if thread shutdown requested
 
 void simuSetKey(uint8_t key, bool state);
 void simuSetTrim(uint8_t trim, bool state);

radio/src/tasks.cpp

@@ -64,7 +64,7 @@ static void menusTask()
   mixerTaskInit();
 
 #if defined(PWR_BUTTON_PRESS)
-  while (true) {
+  while (task_running()) {
     uint32_t pwr_check = pwrCheck();
     if (pwr_check == e_power_off) {
       break;
@@ -111,7 +111,7 @@ static void audioTask()
 #endif
 
   time_point_t next_tick = time_point_now();
-  while (true) {
+  while (task_running()) {
     DEBUG_TIMER_SAMPLE(debugTimerAudioIterval);
     DEBUG_TIMER_START(debugTimerAudioDuration);
     audioQueue.wakeup();

radio/src/tasks/mixer_task.cpp

@@ -48,7 +48,6 @@ static mutex_handle_t mixerMutex;
 // mixerTaskStart() has been called.
 static bool _mixer_started = false;
 static bool _mixer_running = false;
-static bool _mixer_exit = false;
 
 void mixerTaskLock()
 {
@@ -93,14 +92,9 @@ void mixerTaskStop()
   mixerTaskUnlock();
 }
 
-void mixerTaskExit()
-{
-  _mixer_exit = true;
-}
-
 bool mixerTaskRunning()
 {
-  return _mixer_running && !_mixer_exit;
+  return _mixer_running;
 }
 
 volatile uint16_t timeForcePowerOffPressed = 0;
@@ -144,7 +138,7 @@ void mixerTask()
   gyroInit();
 #endif
 
-  while (!_mixer_exit) {
+  while (task_running()) {
 
     int timeout = 0;
     for (; timeout < MIXER_MAX_PERIOD; timeout += MIXER_FREQUENT_ACTIONS_PERIOD) {

radio/src/tasks/mixer_task.h

@@ -53,15 +53,6 @@ void mixerTaskStart();
 //
 void mixerTaskStop();
 
-
-// exit the mixer forever.
-//
-// This is probably only useful for simulations as it is not needed to really
-// exit the task under any normal conditions. There are no guarantees as to when
-// the exit will happen, only that it will eventually happen.
-//
-void mixerTaskExit();
-
 // hint at whether or not the mixer task should be running
 //
 // Please note: there is no guarantee on anything, this is just returning