From dd68b14f469e299f9bf45ee1204c7badff219e24 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 11 Jun 2019 12:15:27 +1000
Subject: [PATCH] AP_HAL_SITL: move dump_strack_trace into HAL

---
 libraries/AP_HAL_SITL/SITL_cmdline.cpp | 79 +-------------------------
 libraries/AP_HAL_SITL/Util.h           |  1 +
 libraries/AP_HAL_SITL/system.cpp       | 76 +++++++++++++++++++++++++
 3 files changed, 79 insertions(+), 77 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
index 6a84c6b4fa..da0dfd2baa 100644
--- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp
+++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
@@ -31,92 +31,17 @@
 
 #include <signal.h>
 #include <stdio.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <unistd.h>
 
 extern const AP_HAL::HAL& hal;
 
 using namespace HALSITL;
 using namespace SITL;
 
-// partly flogged from: https://github.com/tridge/junkcode/blob/master/segv_handler/segv_handler.c
-static void dump_stack_trace()
-{
-    // find dumpstack command:
-    const char *dumpstack = "dumpstack"; // if we can't find it trust in PATH
-    struct stat statbuf;
-    const char *paths[] {
-        "Tools/scripts/dumpstack",
-        "APM/Tools/scripts/dumpstack", // for autotest server
-    };
-    for (uint8_t i=0; i<ARRAY_SIZE(paths); i++) {
-        if (::stat(paths[i], &statbuf) != -1) {
-            dumpstack = paths[i];
-            break;
-        }
-    }
-
-    char cmd[100];
-	char progname[100];
-	char *p;
-	int n;
-
-	n = readlink("/proc/self/exe", progname, sizeof(progname));
-	progname[n] = 0;
-
-	p = strrchr(progname, '/');
-	*p = 0;
-
-    char output_filepath[30];
-    snprintf(output_filepath,
-             ARRAY_SIZE(output_filepath),
-             "segv_%s.%d.out",
-             p+1,
-             (int)getpid());
-	snprintf(cmd,
-             sizeof(cmd),
-             "sh %s %d >%s 2>&1",
-             dumpstack,
-             (int)getpid(),
-             output_filepath);
-    fprintf(stderr, "Running: %s\n", cmd);
-
-	if (system(cmd)) {
-        fprintf(stderr, "Failed\n");
-        return;
-    }
-    fprintf(stderr, "Stack dumped\n");
-
-    // print the trace on stderr:
-    int fd = open(output_filepath, O_RDONLY);
-    if (fd == -1) {
-        fprintf(stderr, "Failed to open stack dump filepath: %m");
-        return;
-    }
-    char buf[1024]; // let's hope we're not here because we ran out of stack
-    while (true) {
-        const ssize_t ret = read(fd, buf, ARRAY_SIZE(buf));
-        if (ret == -1) {
-            fprintf(stderr, "Read error: %m");
-            break;
-        }
-        if (ret == 0) {
-            break;
-        }
-        if (write(2, buf, ret) != ret) {
-            // *sigh*
-            break;
-        }
-    }
-    close(fd);
-}
-
 // catch floating point exceptions
 static void _sig_fpe(int signum)
 {
     fprintf(stderr, "ERROR: Floating point exception - aborting\n");
-    dump_stack_trace();
+    AP_HAL::dump_stack_trace();
     abort();
 }
 
@@ -124,7 +49,7 @@ static void _sig_fpe(int signum)
 static void _sig_segv(int signum)
 {
     fprintf(stderr, "ERROR: segmentation fault - aborting\n");
-    dump_stack_trace();
+    AP_HAL::dump_stack_trace();
     abort();
 }
 
diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h
index d7fe758463..1df3dd06b2 100644
--- a/libraries/AP_HAL_SITL/Util.h
+++ b/libraries/AP_HAL_SITL/Util.h
@@ -32,6 +32,7 @@ public:
 
     bool get_system_id(char buf[40]) override;
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override;
+    void dump_stack_trace();
 
 #ifdef ENABLE_HEAP
     // heap functions, note that a heap once alloc'd cannot be dealloc'd
diff --git a/libraries/AP_HAL_SITL/system.cpp b/libraries/AP_HAL_SITL/system.cpp
index 17d81acd5e..3ea3aecaae 100644
--- a/libraries/AP_HAL_SITL/system.cpp
+++ b/libraries/AP_HAL_SITL/system.cpp
@@ -1,5 +1,9 @@
 #include <stdarg.h>
 #include <stdio.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <fcntl.h>
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/system.h>
@@ -39,6 +43,78 @@ void panic(const char *errormsg, ...)
     for(;;);
 }
 
+// partly flogged from: https://github.com/tridge/junkcode/blob/master/segv_handler/segv_handler.c
+void dump_stack_trace()
+{
+    // find dumpstack command:
+    const char *dumpstack = "dumpstack"; // if we can't find it trust in PATH
+    struct stat statbuf;
+    const char *paths[] {
+        "Tools/scripts/dumpstack",
+        "APM/Tools/scripts/dumpstack", // for autotest server
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(paths); i++) {
+        if (::stat(paths[i], &statbuf) != -1) {
+            dumpstack = paths[i];
+            break;
+        }
+    }
+
+    char cmd[100];
+	char progname[100];
+	char *p;
+	int n;
+
+	n = readlink("/proc/self/exe", progname, sizeof(progname));
+	progname[n] = 0;
+
+	p = strrchr(progname, '/');
+	*p = 0;
+
+    char output_filepath[30];
+    snprintf(output_filepath,
+             ARRAY_SIZE(output_filepath),
+             "segv_%s.%d.out",
+             p+1,
+             (int)getpid());
+	snprintf(cmd,
+             sizeof(cmd),
+             "sh %s %d >%s 2>&1",
+             dumpstack,
+             (int)getpid(),
+             output_filepath);
+    fprintf(stderr, "Running: %s\n", cmd);
+
+	if (system(cmd)) {
+        fprintf(stderr, "Failed\n");
+        return;
+    }
+    fprintf(stderr, "Stack dumped\n");
+
+    // print the trace on stderr:
+    int fd = open(output_filepath, O_RDONLY);
+    if (fd == -1) {
+        fprintf(stderr, "Failed to open stack dump filepath: %m");
+        return;
+    }
+    char buf[1024]; // let's hope we're not here because we ran out of stack
+    while (true) {
+        const ssize_t ret = read(fd, buf, ARRAY_SIZE(buf));
+        if (ret == -1) {
+            fprintf(stderr, "Read error: %m");
+            break;
+        }
+        if (ret == 0) {
+            break;
+        }
+        if (write(2, buf, ret) != ret) {
+            // *sigh*
+            break;
+        }
+    }
+    close(fd);
+}
+
 uint32_t micros()
 {
     return micros64() & 0xFFFFFFFF;