6 files changed, 28 insertions, 14 deletions

diff --git a/firmware/target/sh/archos/fm_v2/power-fm_v2.c b/firmware/target/sh/archos/fm_v2/power-fm_v2.c
index 94a36339bb..ac23348d42 100644
--- a/firmware/target/sh/archos/fm_v2/power-fm_v2.c
+++ b/firmware/target/sh/archos/fm_v2/power-fm_v2.c
@@ -101,9 +101,8 @@ bool ide_powered(void)
 
 void power_off(void)
 {
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     and_b(~0x20, &PBDRL);
     or_b(0x20, &PBIORL);
-    while(1)
-        yield();
+    while(1);
 }
diff --git a/firmware/target/sh/archos/ondio/power-ondio.c b/firmware/target/sh/archos/ondio/power-ondio.c
index 156516afeb..87f365791b 100644
--- a/firmware/target/sh/archos/ondio/power-ondio.c
+++ b/firmware/target/sh/archos/ondio/power-ondio.c
@@ -66,13 +66,12 @@ void power_init(void)
 
 void power_off(void)
 {
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
 #ifdef HAVE_BACKLIGHT
     /* Switch off the light on backlight-modded Ondios */
     _backlight_off();
 #endif
     and_b(~0x20, &PBDRL);
     or_b(0x20, &PBIORL);
-    while(1)
-        yield();
+    while(1);
 }
diff --git a/firmware/target/sh/archos/player/power-player.c b/firmware/target/sh/archos/player/power-player.c
index 7d9d0d7d16..b2f51b9280 100644
--- a/firmware/target/sh/archos/player/power-player.c
+++ b/firmware/target/sh/archos/player/power-player.c
@@ -79,9 +79,8 @@ bool ide_powered(void)
 
 void power_off(void)
 {
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     and_b(~0x08, &PADRH);
     or_b(0x08, &PAIORH);
-    while(1)
-        yield();
+    while(1);
 }
diff --git a/firmware/target/sh/archos/recorder/power-recorder.c b/firmware/target/sh/archos/recorder/power-recorder.c
index 2af8df1bb6..1804bcb660 100644
--- a/firmware/target/sh/archos/recorder/power-recorder.c
+++ b/firmware/target/sh/archos/recorder/power-recorder.c
@@ -95,9 +95,8 @@ bool ide_powered(void)
 
 void power_off(void)
 {
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     and_b(~0x10, &PBDRL);
     or_b(0x10, &PBIORL);
-    while(1)
-        yield();
+    while(1);
 }
diff --git a/firmware/target/sh/system-sh.c b/firmware/target/sh/system-sh.c
index eec3ec18e4..f763e0ff53 100644
--- a/firmware/target/sh/system-sh.c
+++ b/firmware/target/sh/system-sh.c
@@ -360,7 +360,7 @@ void system_init(void)
 
 void system_reboot (void)
 {
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
 
     asm volatile ("ldc\t%0,vbr" : : "r"(0));
 
diff --git a/firmware/target/sh/system-target.h b/firmware/target/sh/system-target.h
index 7fb8fecb6b..d641076694 100644
--- a/firmware/target/sh/system-target.h
+++ b/firmware/target/sh/system-target.h
@@ -52,11 +52,29 @@ static inline int set_irq_level(int level)
 {
     int i;
     /* Read the old level and set the new one */
-    asm volatile ("stc sr, %0" : "=r" (i));
+
+    /* Not volatile - will be optimized away if the return value isn't used */
+    asm ("stc sr, %0" : "=r" (i));
     asm volatile ("ldc %0, sr" : : "r" (level));
     return i;
 }
 
+static inline void enable_irq(void)
+{
+    int i;
+    asm volatile ("mov %1, %0 \n" /* Save a constant load from RAM */
+                  "ldc %0, sr \n" : "=&r"(i) : "i"(0));
+}
+
+#define disable_irq() \
+    ((void)set_irq_level(HIGHEST_IRQ_LEVEL))
+
+#define disable_irq_save() \
+    set_irq_level(HIGHEST_IRQ_LEVEL)
+
+#define restore_irq(i) \
+    ((void)set_irq_level(i))
+
 static inline uint16_t swap16(uint16_t value)
   /*
     result[15..8] = value[ 7..0];