mbv: small refactor interrupts

mbv/hal/interrupts.cc

@@ -1,5 +1,6 @@
 #include <cstdint>
 
+#include "bios.h"
 #include "interrupts.h"
 
 namespace {
@@ -26,27 +27,40 @@ void TrapHandler() {
 
         mip &= ~(kMieExternalInterruptMask);
         asm volatile("csrw mip, %0" :: "r"(mip));
+    } else {
+        BiosWozmon();
     }
 }
 
-}
+}  // namespace
 
 void SetExternalInterruptHandler(Isr handler) {
     external_handler = handler;
 }
 
 void EnableExternalInterrupts() {
-    uint32_t mstatus;
     uint32_t mie;
     Isr trap = TrapHandler;
 
-    asm volatile("csrr %0, mstatus" : "=r"(mstatus));
     asm volatile("csrr %0, mie" : "=r"(mie));
-
     asm volatile("csrw mtvec, %0" :: "r"(trap));
-
     mie |= kMieExternalInterruptMask;
     asm volatile("csrw mie, %0" :: "r"(mie));
-    mstatus |= kMstatusMieMask;
-    asm volatile("csrw mstatus, %0" :: "r"(mstatus));
+}
+
+bool EnableInterrupts(bool on) {
+    uint32_t mstatus;
+    bool was_on;
+    asm volatile("csrr %0, mstatus" : "=r"(mstatus));
+
+    was_on = (mstatus & kMstatusMieMask) > 0;
+
+    if (on) {
+        mstatus |= kMstatusMieMask;
+    } else {
+        mstatus &= ~kMstatusMieMask;
+    }
+
+    asm volatile("csrw mstatus, %0" :: "r"(mstatus));
+    return was_on;
 }