From 51c40a013b5892399e30c93c5eef0482ed3cb63e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 28 Mar 2018 19:49:51 +1100
Subject: [PATCH] HAL_ChibiOS: added IRQ save/restore to hal.scheduler

these are used by RPM driver
---
 libraries/AP_HAL_ChibiOS/Scheduler.cpp | 18 ++++++++++++++++++
 libraries/AP_HAL_ChibiOS/Scheduler.h   | 12 ++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 41cf5dd5d3..71ea7b75b8 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -416,4 +416,22 @@ void Scheduler::system_initialized()
     _initialized = true;
 }
 
+/*
+  disable interrupts and return a context that can be used to
+  restore the interrupt state. This can be used to protect
+  critical regions
+*/
+void *Scheduler::disable_interrupts_save(void)
+{
+    return (void *)(uintptr_t)chSysGetStatusAndLockX();
+}
+
+/*
+  restore interrupt state from disable_interrupts_save()
+*/
+void Scheduler::restore_interrupts(void *state)
+{
+    chSysRestoreStatusX((syssts_t)(uintptr_t)state);
+}
+
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index bf2a6dc476..904b4f3aa9 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -75,6 +75,18 @@ public:
     void     hal_initialized() { _hal_initialized = true; }
 
     bool     check_called_boost(void);
+
+    /*
+      disable interrupts and return a context that can be used to
+      restore the interrupt state. This can be used to protect
+      critical regions
+     */
+    void *disable_interrupts_save(void) override;
+
+    /*
+      restore interrupt state from disable_interrupts_save()
+     */
+    void restore_interrupts(void *) override;
     
 private:
     bool _initialized;