msp430fr5969: sleep: restore interrupt state

src/arch/msp430fr5969lp/arch.cc

@@ -86,6 +86,13 @@ extern void loop();
 volatile char run_loop = 0;
 #endif
 
+inline static unsigned int irq_enabled()
+{
+	unsigned int sr;
+	__asm__ __volatile__("mov SR, %0" : "=r" (sr) : );
+	return sr & GIE;
+}
+
 volatile bool sleep_done = false;
 
 // max delay: 262 ms @ 16 MHz
@@ -95,6 +102,7 @@ void Arch::sleep_ms(unsigned int const ms)
 	if (ms == 0) {
 		return;
 	}
+	int int_enabled = irq_enabled();
 	sleep_done = false;
 #if F_CPU == 16000000UL
 	TA3CTL = TASSEL__SMCLK | ID__8; // /8
@@ -121,7 +129,9 @@ void Arch::sleep_ms(unsigned int const ms)
 		asm volatile("nop");
 		__bis_SR_register(GIE | LPM2_bits);
 		asm volatile("nop");
-		__dint();
+		if (!int_enabled) {
+			__dint();
+		}
 	}
 	TA3CTL = TASSEL__SMCLK;
 }