From b0193ca45ec017448020ef9add9b0205c075b2cd Mon Sep 17 00:00:00 2001
From: Thomas Klaehn <thomas.klaehn@perinet.io>
Date: Sun, 12 Apr 2020 09:33:53 +0200
Subject: [PATCH] Add interrupt lock

---
 src/application/sys_tick/main.cc      |  4 +++-
 src/platform/cm4/InterruptGuardian.cc |  1 +
 src/platform/cm4/InterruptGuardian.h  | 14 ++++++++++++++
 src/platform/cm4/InterruptLock.cc     | 15 +++++++++++++++
 src/platform/cm4/InterruptLock.h      | 18 ++++++++++++++++++
 5 files changed, 51 insertions(+), 1 deletion(-)
 create mode 100644 src/platform/cm4/InterruptLock.cc
 create mode 100644 src/platform/cm4/InterruptLock.h

diff --git a/src/application/sys_tick/main.cc b/src/application/sys_tick/main.cc
index 75d27a6..2c8e756 100644
--- a/src/application/sys_tick/main.cc
+++ b/src/application/sys_tick/main.cc
@@ -33,9 +33,11 @@ int main(void)
     cm4::SystemTick ticker;
     ticker.enable();
     uint32_t last_tick = 0;
+
+    cm4::InterruptGuardian::enable_interrupts();
     while(true) {
         for(auto it = std::begin(leds); it != std::end(leds); ++it) {
-            uint32_t tick = ticker.tick() / 1000;
+            volatile uint32_t tick = ticker.tick() / 1000;
             if(tick != last_tick) {
                 nrf52::Gpio * tmp = *it;
                 tmp->toggle();
diff --git a/src/platform/cm4/InterruptGuardian.cc b/src/platform/cm4/InterruptGuardian.cc
index 6a4bec9..b8753e3 100644
--- a/src/platform/cm4/InterruptGuardian.cc
+++ b/src/platform/cm4/InterruptGuardian.cc
@@ -5,6 +5,7 @@ using namespace pinetime::platform::cm4;
 
 InterruptGuardian::InterruptGuardian()
 {
+    disable_interrupts();
 }
 
 void InterruptGuardian::register_handler(Cm4IrqN irq_nr, InterruptHandler &handler)
diff --git a/src/platform/cm4/InterruptGuardian.h b/src/platform/cm4/InterruptGuardian.h
index edd5047..3d6dbe5 100644
--- a/src/platform/cm4/InterruptGuardian.h
+++ b/src/platform/cm4/InterruptGuardian.h
@@ -33,6 +33,20 @@ private:
     };
 
 public:
+    static inline void enable_interrupts() { asm volatile("cpsie i"); }
+    static inline void disable_interrupts() { asm volatile("cpsid i"); }
+    static inline bool suspend_interrupts()
+    {
+        uint32_t mask;
+        asm volatile("mrs %0, primask" : "=r" (mask));
+        asm volatile("cpsid i");
+        return(mask & 0x01);
+    }
+    static inline void resume_interrupts(bool status)
+    {
+        asm volatile ("msr primask, %0" : : "r" (status) : "memory");
+    }
+
     void register_handler(Cm4IrqN irq_nr, InterruptHandler &);
 
     static InterruptGuardian instance;
diff --git a/src/platform/cm4/InterruptLock.cc b/src/platform/cm4/InterruptLock.cc
new file mode 100644
index 0000000..ecba21f
--- /dev/null
+++ b/src/platform/cm4/InterruptLock.cc
@@ -0,0 +1,15 @@
+#include "platform/cm4/InterruptHandler.h"
+#include "platform/cm4/InterruptGuardian.h"
+#include "platform/cm4/InterruptLock.h"
+
+using namespace pinetime::platform::cm4;
+
+InterruptLock::InterruptLock()
+{
+    this->state = InterruptGuardian::suspend_interrupts();
+}
+
+InterruptLock::~InterruptLock()
+{
+    InterruptGuardian::resume_interrupts(this->state);
+}
diff --git a/src/platform/cm4/InterruptLock.h b/src/platform/cm4/InterruptLock.h
new file mode 100644
index 0000000..d2168c4
--- /dev/null
+++ b/src/platform/cm4/InterruptLock.h
@@ -0,0 +1,18 @@
+#ifndef __PLATFORM_CM$_INTERRUPTLOCK_H__
+#define __PLATFORM_CM$_INTERRUPTLOCK_H__
+
+namespace pinetime::platform::cm4 {
+
+class InterruptLock
+{
+public:
+    InterruptLock();
+    ~InterruptLock();
+
+private:
+    bool state;
+};
+
+}
+
+#endif