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;