AP_HAL_SITL: avoid lockstep scheduling issue with semaphores

If there is another thread holding a lock that the main thread wants to
take while pretending to be the IO thread, we will wait forever as we do
not move SITL time forward while pretending to be the IO thread.

This patch simply allows time to move forward if we've failed to take a
semaphore immediately and need to wait.

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -204,7 +204,8 @@ void SITL_State::_fdm_input_step(void)
 void SITL_State::wait_clock(uint64_t wait_time_usec)
 {
     while (AP_HAL::micros64() < wait_time_usec) {
-        if (hal.scheduler->in_main_thread()) {
+        if (hal.scheduler->in_main_thread() ||
+            Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
             _fdm_input_step();
         } else {
             usleep(1000);

libraries/AP_HAL_SITL/Scheduler.cpp

@@ -29,6 +29,8 @@ uint8_t Scheduler::_num_io_procs = 0;
 bool Scheduler::_in_io_proc = false;
 bool Scheduler::_should_reboot = false;
 
+bool Scheduler::_in_semaphore_take_wait = false;
+
 Scheduler::thread_attr *Scheduler::threads;
 HAL_Semaphore Scheduler::_thread_sem;
 
@@ -51,6 +53,27 @@ bool Scheduler::in_main_thread() const
     return false;
 }
 
+/*
+ * semaphore_wait_hack_required - possibly move time input step
+ * forward even if we are currently pretending to be the IO or timer
+ * threads.
+ *
+ * Without this, if another thread has taken a semaphore (e.g. the
+ * Object Avoidance thread), and an "IO process" tries to take that
+ * semaphore with a timeout specified, then we end up not advancing
+ * time (due to the logic in SITL_State::wait_clock) and thus taking
+ * the semaphore never times out - meaning we essentially deadlock.
+ */
+bool Scheduler::semaphore_wait_hack_required()
+{
+    if (pthread_self() != _main_ctx) {
+        // only the main thread ever moves stuff forwards
+        return false;
+    }
+
+    return _in_semaphore_take_wait;
+}
+
 void Scheduler::delay_microseconds(uint16_t usec)
 {
     uint64_t start = AP_HAL::micros64();

libraries/AP_HAL_SITL/Scheduler.h

@@ -56,7 +56,16 @@ public:
      */
     bool thread_create(AP_HAL::MemberProc, const char *name,
                        uint32_t stack_size, priority_base base, int8_t priority) override;
-    
+
+    void set_in_semaphore_take_wait(bool value) { _in_semaphore_take_wait = value; }
+    /*
+     * semaphore_wait_hack_required - possibly move time input step
+     * forward even if we are currently pretending to be the IO or timer
+     * threads.
+     */
+    // a couple of helper functions to cope with SITL's time stepping
+    bool semaphore_wait_hack_required();
+
 private:
     SITL_State *_sitlState;
     uint8_t _nested_atomic_ctr;
@@ -72,6 +81,10 @@ private:
     static bool _in_timer_proc;
     static bool _in_io_proc;
 
+    // boolean set by the Semaphore code to indicate it's currently
+    // waiting for a take-timeout to occur.
+    static bool _in_semaphore_take_wait;
+
     void stop_clock(uint64_t time_usec) override;
 
     static void *thread_create_trampoline(void *ctx);

libraries/AP_HAL_SITL/Semaphores.cpp

@@ -3,6 +3,7 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 #include "Semaphores.h"
+#include "Scheduler.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -42,7 +43,9 @@ bool Semaphore::take(uint32_t timeout_ms)
     }
     uint64_t start = AP_HAL::micros64();
     do {
+        Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(true);
         hal.scheduler->delay_microseconds(200);
+        Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(false);
         if (take_nonblocking()) {
             return true;
         }