From ee9d1611964815006dba7ae0d7c2621e61d1a478 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 11 May 2020 15:55:43 +1000
Subject: [PATCH] HAL_SITL: resync for 4.0 update

---
 libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h |  1 -
 libraries/AP_HAL_SITL/HAL_SITL_Class.cpp      |  6 ++++--
 libraries/AP_HAL_SITL/Scheduler.cpp           | 15 +++++++++++++--
 libraries/AP_HAL_SITL/Semaphores.cpp          |  6 ------
 libraries/AP_HAL_SITL/Semaphores.h            |  8 --------
 5 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
index ae64967618..26bb798f44 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
@@ -12,7 +12,6 @@ class ADCSource;
 class RCInput;
 class Util;
 class Semaphore;
-class Semaphore_Recursive;
 class GPIO;
 class DigitalSource;
 class HALSITLCAN;
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
index fc2c4a7511..b013909249 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -162,9 +162,10 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
     analogin->init();
 
     if (getenv("SITL_WATCHDOG_RESET")) {
-        AP::internalerror().error(AP_InternalError::error_t::watchdog_reset);
+        INTERNAL_ERROR(AP_InternalError::error_t::watchdog_reset);
         if (watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4)) {
             uartA->printf("Loaded watchdog data");
+            utilInstance.last_persistent_data = utilInstance.persistent_data;
         }
     }
 
@@ -185,11 +186,12 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
 
     if (getenv("SITL_WATCHDOG_RESET")) {
         const AP_HAL::Util::PersistentData &pd = util->persistent_data;
-        AP::logger().WriteCritical("WDOG", "TimeUS,Task,IErr,IErrCnt,MavMsg,MavCmd,SemLine", "QbIIHHH",
+        AP::logger().WriteCritical("WDOG", "TimeUS,Task,IErr,IErrCnt,IErrLn,MavMsg,MavCmd,SemLine", "QbIHHHHH",
                                    AP_HAL::micros64(),
                                    pd.scheduler_task,
                                    pd.internal_errors,
                                    pd.internal_error_count,
+                                   pd.internal_error_last_line,
                                    pd.last_mavlink_msgid,
                                    pd.last_mavlink_cmd,
                                    pd.semaphore_line);
diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp
index 2667f4ff48..c37700a78c 100644
--- a/libraries/AP_HAL_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_SITL/Scheduler.cpp
@@ -17,6 +17,13 @@ using namespace HALSITL;
 
 extern const AP_HAL::HAL& hal;
 
+#ifndef SITL_STACK_CHECKING_ENABLED
+//#define SITL_STACK_CHECKING_ENABLED !defined(__CYGWIN__) && !defined(__CYGWIN64__)
+// stack checking is disabled until the memory corruption issues are
+// fixed with pthread_attr_setstack.  These may be due to
+// changes in the way guard pages are handled.
+#define SITL_STACK_CHECKING_ENABLED 0
+#endif
 
 AP_HAL::Proc Scheduler::_failsafe = nullptr;
 
@@ -233,7 +240,9 @@ void Scheduler::_run_io_procs()
     hal.uartH->_timer_tick();
     hal.storage->_timer_tick();
 
+#if SITL_STACK_CHECKING_ENABLED
     check_thread_stacks();
+#endif
 
     AP::RC().update();
 }
@@ -315,8 +324,10 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
     a->f[0] = proc;
     a->name = name;
     
-    pthread_attr_init(&a->attr);
-#if !defined(__CYGWIN__) && !defined(__CYGWIN64__)
+    if (pthread_attr_init(&a->attr) != 0) {
+        goto failed;
+    }
+#if SITL_STACK_CHECKING_ENABLED
     if (pthread_attr_setstack(&a->attr, a->stack, alloc_stack) != 0) {
         AP_HAL::panic("Failed to set stack of size %u for thread %s", alloc_stack, name);
     }
diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp
index 48e964a778..cdefa81990 100644
--- a/libraries/AP_HAL_SITL/Semaphores.cpp
+++ b/libraries/AP_HAL_SITL/Semaphores.cpp
@@ -11,12 +11,6 @@ using namespace HALSITL;
 
 // construct a semaphore
 Semaphore::Semaphore()
-{
-    pthread_mutex_init(&_lock, nullptr);
-}
-
-// construct a recursive semaphore (allows a thread to take it more than once)
-Semaphore_Recursive::Semaphore_Recursive()
 {
     pthread_mutexattr_t attr;
     pthread_mutexattr_init(&attr);
diff --git a/libraries/AP_HAL_SITL/Semaphores.h b/libraries/AP_HAL_SITL/Semaphores.h
index 0fdb4589c7..f19a566650 100644
--- a/libraries/AP_HAL_SITL/Semaphores.h
+++ b/libraries/AP_HAL_SITL/Semaphores.h
@@ -16,11 +16,3 @@ public:
 protected:
     pthread_mutex_t _lock;
 };
-
-
-class HALSITL::Semaphore_Recursive : public HALSITL::Semaphore {
-public:
-    Semaphore_Recursive();
-};
-
-