From 89420e4b2da428b1f1878210e51ada0292ceb5ee Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 26 Oct 2016 19:27:12 -0200
Subject: [PATCH] AP_HAL_Linux: add signal handler for normal termination

This allows to terminate the flight stack nicely, ensuring it returns 0
so init system can check by return code if it terminated nicely or if it
was due to a crash.
---
 libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 32 +++++++++++++++++++---
 libraries/AP_HAL_Linux/HAL_Linux_Class.h   |  7 +++++
 libraries/AP_HAL_Linux/RCInput_RPI.cpp     |  4 ++-
 3 files changed, 38 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
index 24cb2ec5cc..0744c0baf8 100644
--- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
+++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
@@ -1,6 +1,7 @@
 #include "HAL_Linux_Class.h"
 
 #include <assert.h>
+#include <signal.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
@@ -348,6 +349,8 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
         }
     }
 
+    setup_signal_handlers();
+
     scheduler->init();
     gpio->init();
     rcout->init();
@@ -372,12 +375,33 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
     callbacks->setup();
     AP_Module::call_hook_setup_complete();
 
-    for (;;) {
+    while (!_should_exit) {
         callbacks->loop();
     }
+
+    rcin->teardown();
+    I2CDeviceManager::from(i2c_mgr)->teardown();
+    SPIDeviceManager::from(spi)->teardown();
 }
 
-const AP_HAL::HAL& AP_HAL::get_HAL() {
-    static const HAL_Linux hal;
-    return hal;
+void HAL_Linux::setup_signal_handlers() const
+{
+    struct sigaction sa = { };
+
+    sa.sa_flags = SA_NOCLDSTOP;
+    sa.sa_handler = HAL_Linux::exit_signal_handler;
+    sigaction(SIGTERM, &sa, NULL);
+    sigaction(SIGINT, &sa, NULL);
+}
+
+static HAL_Linux halInstance;
+
+void HAL_Linux::exit_signal_handler(int signum)
+{
+    halInstance._should_exit = true;
+}
+
+const AP_HAL::HAL &AP_HAL::get_HAL()
+{
+    return halInstance;
 }
diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.h b/libraries/AP_HAL_Linux/HAL_Linux_Class.h
index 9aa95ddc86..ec45662c9d 100644
--- a/libraries/AP_HAL_Linux/HAL_Linux_Class.h
+++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.h
@@ -6,4 +6,11 @@ class HAL_Linux : public AP_HAL::HAL {
 public:
     HAL_Linux();
     void run(int argc, char* const* argv, Callbacks* callbacks) const override;
+
+    void setup_signal_handlers() const;
+
+    static void exit_signal_handler(int);
+
+protected:
+    bool _should_exit = false;
 };
diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp
index e090e535ed..339c34d7d2 100644
--- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp
+++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp
@@ -379,7 +379,9 @@ void RCInput_RPI::init_DMA()
 void RCInput_RPI::set_sigaction()
 {
     for (int i = 0; i < NSIG; i++) {
-        //catch all signals (like ctrl+c, ctrl+z, ...) to ensure DMA is disabled
+        // catch all signals to ensure DMA is disabled - some of them may
+        // already be handled elsewhere in cases we consider normal
+        // termination. In those cases the teardown() method must be called.
         struct sigaction sa, sa_old;
         memset(&sa, 0, sizeof(sa));
         sigaction(i, nullptr, &sa_old);