50 files changed, 337 insertions, 248 deletions

diff --git a/firmware/drivers/audio/uda1380.c b/firmware/drivers/audio/uda1380.c
index ce69ef6b80..f6a7c730cb 100644
--- a/firmware/drivers/audio/uda1380.c
+++ b/firmware/drivers/audio/uda1380.c
@@ -203,10 +203,10 @@ void audiohw_enable_output(bool enable)
 static void reset(void)
 {
 #ifdef IRIVER_H300_SERIES
-    int mask = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int mask = disable_irq_save();
     pcf50606_write(0x3b, 0x00);  /* GPOOD2 high Z */
     pcf50606_write(0x3b, 0x07);  /* GPOOD2 low */
-    set_irq_level(mask);
+    restore_irq(mask);
 #else
     /* RESET signal */
     or_l(1<<29, &GPIO_OUT);
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index b9473bec24..d38306b3ca 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -465,10 +465,10 @@ void button_set_flip(bool flip)
     if (flip != flipped) /* not the current setting */
     {
         /* avoid race condition with the button_tick() */
-        int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        int oldlevel = disable_irq_save();
         lastbtn = button_flip(lastbtn);
         flipped = flip;
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
     }
 }
 #endif /* HAVE_LCD_BITMAP */
diff --git a/firmware/drivers/rtc/rtc_pcf50606.c b/firmware/drivers/rtc/rtc_pcf50606.c
index 25b0c704c0..9b4de44d3f 100644
--- a/firmware/drivers/rtc/rtc_pcf50606.c
+++ b/firmware/drivers/rtc/rtc_pcf50606.c
@@ -30,21 +30,21 @@ void rtc_init(void)
 
 int rtc_read_datetime(unsigned char* buf) {
     int rc;
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     
     rc = pcf50606_read_multiple(0x0a, buf, 7);
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
     return rc;
 }
 
 int rtc_write_datetime(unsigned char* buf) {
     int rc;
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     
     rc = pcf50606_write_multiple(0x0a, buf, 7);
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     return rc;
 }
diff --git a/firmware/kernel.c b/firmware/kernel.c
index 47c0d58a95..be0cfb56a4 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -270,7 +270,7 @@ void tick_start(unsigned int interval_in_ms)
 int tick_add_task(void (*f)(void))
 {
     int i;
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     /* Add a task if there is room */
     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
@@ -278,11 +278,11 @@ int tick_add_task(void (*f)(void))
         if(tick_funcs[i] == NULL)
         {
             tick_funcs[i] = f;
-            set_irq_level(oldlevel);
+            restore_irq(oldlevel);
             return 0;
         }
     }
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
     panicf("Error! tick_add_task(): out of tasks");
     return -1;
 }
@@ -290,7 +290,7 @@ int tick_add_task(void (*f)(void))
 int tick_remove_task(void (*f)(void))
 {
     int i;
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     /* Remove a task if it is there */
     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
@@ -298,12 +298,12 @@ int tick_remove_task(void (*f)(void))
         if(tick_funcs[i] == f)
         {
             tick_funcs[i] = NULL;
-            set_irq_level(oldlevel);
+            restore_irq(oldlevel);
             return 0;
         }
     }
     
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
     return -1;
 }
 
@@ -341,7 +341,7 @@ static void timeout_tick(void)
 /* Cancels a timeout callback - can be called from the ISR */
 void timeout_cancel(struct timeout *tmo)
 {
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     if (tmo_list != NULL)
     {
@@ -368,7 +368,7 @@ void timeout_cancel(struct timeout *tmo)
         /* not in list or tmo == NULL */
     }
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 /* Adds a timeout callback - calling with an active timeout resets the
@@ -382,7 +382,7 @@ void timeout_register(struct timeout *tmo, timeout_cb_type callback,
     if (tmo == NULL)
         return;
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
 
     /* see if this one is already registered */
     curr = tmo_list;
@@ -404,7 +404,7 @@ void timeout_register(struct timeout *tmo, timeout_cb_type callback,
     tmo->data = data;
     *(long *)&tmo->expires = current_tick + ticks;
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 #endif /* INCLUDE_TIMEOUT_API */
@@ -433,7 +433,7 @@ void sleep(int ticks)
     while (TIME_BEFORE(USEC_TIMER, stop))
         switch_thread();
 #else
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     sleep_thread(ticks);
     switch_thread();
 #endif
@@ -537,7 +537,7 @@ void queue_enable_queue_send(struct event_queue *q,
                              struct queue_sender_list *send,
                              struct thread_entry *owner)
 {
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     if(send != NULL && q->send == NULL)
@@ -554,7 +554,7 @@ void queue_enable_queue_send(struct event_queue *q,
     }
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     (void)owner;
 }
@@ -618,7 +618,7 @@ static inline void queue_do_fetch_sender(struct queue_sender_list *send,
 /* Queue must not be available for use during this call */
 void queue_init(struct event_queue *q, bool register_queue)
 {
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     if(register_queue)
     {
@@ -645,7 +645,7 @@ void queue_init(struct event_queue *q, bool register_queue)
         corelock_unlock(&all_queues.cl);
     }
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 /* Queue must not be available for use during this call */
@@ -654,7 +654,7 @@ void queue_delete(struct event_queue *q)
     int oldlevel;
     int i;
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&all_queues.cl);
     corelock_lock(&q->cl);
 
@@ -697,7 +697,7 @@ void queue_delete(struct event_queue *q)
     q->write = 0;
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 /* NOTE: multiple threads waiting on a queue head cannot have a well-
@@ -714,7 +714,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
                   "queue_wait->wrong thread\n");
 #endif
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     /* auto-reply */
@@ -734,7 +734,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
             corelock_unlock(&q->cl);
             switch_thread();
 
-            oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+            oldlevel = disable_irq_save();
             corelock_lock(&q->cl);
         }
         /* A message that woke us could now be gone */
@@ -748,7 +748,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
     queue_do_fetch_sender(q->send, rd);
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
@@ -761,7 +761,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
                   "queue_wait_w_tmo->wrong thread\n");
 #endif
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     /* Auto-reply */
@@ -779,7 +779,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 
         switch_thread();
 
-        oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        oldlevel = disable_irq_save();
         corelock_lock(&q->cl);
     }
 
@@ -798,7 +798,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
     }
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 void queue_post(struct event_queue *q, long id, intptr_t data)
@@ -806,7 +806,7 @@ void queue_post(struct event_queue *q, long id, intptr_t data)
     int oldlevel;
     unsigned int wr;
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     wr = q->write++ & QUEUE_LENGTH_MASK;
@@ -821,7 +821,7 @@ void queue_post(struct event_queue *q, long id, intptr_t data)
     wakeup_thread(&q->queue);
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
@@ -832,7 +832,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
     int oldlevel;
     unsigned int wr;
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     wr = q->write++ & QUEUE_LENGTH_MASK;
@@ -875,7 +875,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
     wakeup_thread(&q->queue);
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
     
     return 0;
 }
@@ -887,7 +887,7 @@ bool queue_in_queue_send(struct event_queue *q)
     bool in_send;
 
 #if NUM_CORES > 1
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 #endif
 
@@ -895,7 +895,7 @@ bool queue_in_queue_send(struct event_queue *q)
 
 #if NUM_CORES > 1
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 #endif
 
     return in_send;
@@ -907,7 +907,7 @@ void queue_reply(struct event_queue *q, intptr_t retval)
 {
     if(q->send && q->send->curr_sender)
     {
-        int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        int oldlevel = disable_irq_save();
         corelock_lock(&q->cl);
         /* Double-check locking */
         IF_COP( if(q->send && q->send->curr_sender) )
@@ -916,7 +916,7 @@ void queue_reply(struct event_queue *q, intptr_t retval)
         }
 
         corelock_unlock(&q->cl);
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
     }
 }
 
@@ -927,7 +927,7 @@ bool queue_peek(struct event_queue *q, struct queue_event *ev)
 
     bool have_msg = false;
 
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     if(q->read != q->write)
@@ -937,7 +937,7 @@ bool queue_peek(struct event_queue *q, struct queue_event *ev)
     }
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     return have_msg;
 }
@@ -956,7 +956,7 @@ void queue_clear(struct event_queue* q)
 {
     int oldlevel;
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     /* Release all threads waiting in the queue for a reply -
@@ -967,14 +967,14 @@ void queue_clear(struct event_queue* q)
     q->write = 0;
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 void queue_remove_from_head(struct event_queue *q, long id)
 {
     int oldlevel;
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
     corelock_lock(&q->cl);
 
     while(q->read != q->write)
@@ -993,7 +993,7 @@ void queue_remove_from_head(struct event_queue *q, long id)
     }
 
     corelock_unlock(&q->cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 /**
@@ -1012,7 +1012,7 @@ int queue_broadcast(long id, intptr_t data)
     int i;
 
 #if NUM_CORES > 1
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     corelock_lock(&all_queues.cl);
 #endif
     
@@ -1023,7 +1023,7 @@ int queue_broadcast(long id, intptr_t data)
 
 #if NUM_CORES > 1
     corelock_unlock(&all_queues.cl);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 #endif
    
     return i;
@@ -1079,7 +1079,7 @@ void mutex_lock(struct mutex *m)
     IF_PRIO( current->blocker = &m->blocker; )
     current->bqp = &m->queue;
 
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     block_thread(current);
 
     corelock_unlock(&m->cl);
@@ -1118,13 +1118,13 @@ void mutex_unlock(struct mutex *m)
     }
     else
     {
-        const int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        const int oldlevel = disable_irq_save();
         /* Tranfer of owning thread is handled in the wakeup protocol
          * if priorities are enabled otherwise just set it from the
          * queue head. */
         IFN_PRIO( MUTEX_SET_THREAD(m, m->queue); )
         IF_PRIO( unsigned int result = ) wakeup_thread(&m->queue);
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
 
         corelock_unlock(&m->cl);
 
@@ -1219,7 +1219,7 @@ void semaphore_wait(struct semaphore *s)
     IF_COP( current->obj_cl = &s->cl; )
     current->bqp = &s->queue;
 
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     block_thread(current);
 
     corelock_unlock(&s->cl);
@@ -1239,9 +1239,9 @@ void semaphore_release(struct semaphore *s)
         /* there should be threads in this queue */
         KERNEL_ASSERT(s->queue != NULL, "semaphore->wakeup\n");
         /* a thread was queued - wake it up */
-        int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        int oldlevel = disable_irq_save();
         IF_PRIO( result = ) wakeup_thread(&s->queue);
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
     }
 
     corelock_unlock(&s->cl);
@@ -1298,7 +1298,7 @@ void event_wait(struct event *e, unsigned int for_state)
     IF_COP( current->obj_cl = &e->cl; )
     current->bqp = &e->queues[for_state];
 
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     block_thread(current);
 
     corelock_unlock(&e->cl);
@@ -1323,7 +1323,7 @@ void event_set_state(struct event *e, unsigned int state)
 
     IF_PRIO( result = THREAD_OK; )
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
 
     if(state == STATE_SIGNALED)
     {
@@ -1357,7 +1357,7 @@ void event_set_state(struct event *e, unsigned int state)
             thread_queue_wake(&e->queues[STATE_NONSIGNALED]);
     }
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     corelock_unlock(&e->cl);
 
diff --git a/firmware/mp3_playback.c b/firmware/mp3_playback.c
index b2b7205318..ed71be290c 100644
--- a/firmware/mp3_playback.c
+++ b/firmware/mp3_playback.c
@@ -134,7 +134,7 @@ static void postpone_dma_tick(void)
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
 void demand_irq_enable(bool on)
 {
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     
     if(on)
     {
@@ -144,7 +144,7 @@ void demand_irq_enable(bool on)
     else
         IPRA &= 0xfff0;
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 #endif /* #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F) */
 
diff --git a/firmware/mpeg.c b/firmware/mpeg.c
index 273f630799..11cbcdcb68 100644
--- a/firmware/mpeg.c
+++ b/firmware/mpeg.c
@@ -1767,9 +1767,9 @@ static void mpeg_thread(void)
                         DEBUGF("New audiobuf_read address: %x (%x)\n",
                                audiobuf+audiobuf_read, audiobuf_read);
 
-                        level = set_irq_level(HIGHEST_IRQ_LEVEL);
+                        level = disable_irq_save();
                         num_rec_bytes = get_unsaved_space();
-                        set_irq_level(level);
+                        restore_irq(level);
                     }
                     else
                     {
@@ -1860,11 +1860,11 @@ static void mpeg_thread(void)
                         pause_start_time = record_start_time;
 
                     /* capture all values at one point */
-                    level = set_irq_level(HIGHEST_IRQ_LEVEL);
+                    level = disable_irq_save();
                     save_endpos = audiobuf_write;
                     last_rec_bytes = num_rec_bytes;
                     num_rec_bytes = 0;
-                    set_irq_level(level);
+                    restore_irq(level);
 
                     if (amount_to_save >= 1800)
                     {
@@ -1883,9 +1883,9 @@ static void mpeg_thread(void)
                             save_endpos -= audiobuflen;
 
                         last_rec_bytes += offset - 1800;
-                        level = set_irq_level(HIGHEST_IRQ_LEVEL);
+                        level = disable_irq_save();
                         num_rec_bytes += 1800 - offset;
-                        set_irq_level(level);
+                        restore_irq(level);
                     }
 
                     saving_status = NEW_FILE;
diff --git a/firmware/rolo.c b/firmware/rolo.c
index eac45e10ce..30eba91316 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -271,7 +271,11 @@ int rolo_load(const char* filename)
 #endif
     adc_close();
 
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+#ifdef CPU_ARM
+    disable_fiq();
+#endif
+    set_irq_level(DISABLE_INTERRUPTS);
+
 #elif CONFIG_CPU == SH7034
     /* Read file length from header and compare to real file length */
     lseek(fd, FIRMWARE_OFFSET_FILE_LENGTH, SEEK_SET);
diff --git a/firmware/target/arm/i2c-pp.c b/firmware/target/arm/i2c-pp.c
index 1cfbfaeff1..450effc32d 100644
--- a/firmware/target/arm/i2c-pp.c
+++ b/firmware/target/arm/i2c-pp.c
@@ -60,7 +60,7 @@ static int pp_i2c_read_byte(unsigned int addr, unsigned int *data)
 
     {
         unsigned int byte;
-        int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+        int old_irq_level = disable_irq_save();
 
         /* clear top 15 bits, left shift 1, or in 0x1 for a read */
         I2C_ADDR = ((addr << 17) >> 16) | 0x1 ;
@@ -69,19 +69,19 @@ static int pp_i2c_read_byte(unsigned int addr, unsigned int *data)
 
         I2C_CTRL |= I2C_SEND;
 
-        set_irq_level(old_irq_level);
+        restore_irq(old_irq_level);
         if (pp_i2c_wait_not_busy() < 0)
         {
             return -1;
         }
-        old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+        old_irq_level = disable_irq_save();
 
         byte = I2C_DATA(0);
 
         if (data)
             *data = byte;
 
-        set_irq_level(old_irq_level);
+        restore_irq(old_irq_level);
     }
 
     return 0;
@@ -102,7 +102,7 @@ static int pp_i2c_send_bytes(unsigned int addr, unsigned int len, unsigned char
     }
 
     {
-        int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+        int old_irq_level = disable_irq_save();
 
         /* clear top 15 bits, left shift 1 */
         I2C_ADDR = (addr << 17) >> 16;
@@ -118,7 +118,7 @@ static int pp_i2c_send_bytes(unsigned int addr, unsigned int len, unsigned char
 
         I2C_CTRL |= I2C_SEND;
 
-        set_irq_level(old_irq_level);
+        restore_irq(old_irq_level);
     }
 
     return 0x0;
diff --git a/firmware/target/arm/ipod/backlight-nano_video.c b/firmware/target/arm/ipod/backlight-nano_video.c
index 6d77e2bd03..647bab9ac6 100644
--- a/firmware/target/arm/ipod/backlight-nano_video.c
+++ b/firmware/target/arm/ipod/backlight-nano_video.c
@@ -43,11 +43,11 @@ void _backlight_set_brightness(int val)
     {
         do
         {
-            oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+            oldlevel = disable_irq_save();
             GPIO_CLEAR_BITWISE(GPIOD_OUTPUT_VAL, 0x80);
             udelay(10);
             GPIO_SET_BITWISE(GPIOD_OUTPUT_VAL, 0x80);
-            set_irq_level(oldlevel);
+            restore_irq(oldlevel);
             udelay(10);
         }
         while (++current_dim < val);
@@ -56,11 +56,11 @@ void _backlight_set_brightness(int val)
     {
         do
         {
-            oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+            oldlevel = disable_irq_save();
             GPIO_CLEAR_BITWISE(GPIOD_OUTPUT_VAL, 0x80);
             udelay(200);
             GPIO_SET_BITWISE(GPIOD_OUTPUT_VAL, 0x80);
-            set_irq_level(oldlevel);
+            restore_irq(oldlevel);
             udelay(10);
         }
         while (--current_dim > val);
diff --git a/firmware/target/arm/ipod/video/lcd-video.c b/firmware/target/arm/ipod/video/lcd-video.c
index d0c82e905c..0e1b072917 100644
--- a/firmware/target/arm/ipod/video/lcd-video.c
+++ b/firmware/target/arm/ipod/video/lcd-video.c
@@ -110,7 +110,7 @@ static void bcm_setup_rect(unsigned x, unsigned y,
 #ifndef BOOTLOADER
 static void lcd_tick(void)
 {
-    /* No set_irq_level - already in interrupt context */
+    /* No core level interrupt mask - already in interrupt context */
 #if NUM_CORES > 1
     corelock_lock(&lcd_state.cl);
 #endif
@@ -143,7 +143,7 @@ static void lcd_tick(void)
 
 static inline void lcd_block_tick(void)
 {
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
 #if NUM_CORES > 1
     corelock_lock(&lcd_state.cl);
@@ -152,14 +152,14 @@ static inline void lcd_block_tick(void)
 #else
     lcd_state.blocked = true;
 #endif
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 static void lcd_unblock_and_update(void)
 {
     unsigned data;
     bool bcm_is_busy;
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
 #if NUM_CORES > 1
     corelock_lock(&lcd_state.cl);
@@ -184,7 +184,7 @@ static void lcd_unblock_and_update(void)
 #if NUM_CORES > 1
     corelock_unlock(&lcd_state.cl);
 #endif
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 #else /* BOOTLOADER */
diff --git a/firmware/target/arm/pcm-pp.c b/firmware/target/arm/pcm-pp.c
index 5a9d3b3670..433e6e1e4f 100644
--- a/firmware/target/arm/pcm-pp.c
+++ b/firmware/target/arm/pcm-pp.c
@@ -223,24 +223,24 @@ void fiq_playback(void)
    will require other measures */
 void pcm_play_lock(void)
 {
-    int status = set_fiq_status(FIQ_DISABLED);
+    int status = disable_fiq_save();
 
     if (++dma_play_data.locked == 1) {
         IIS_IRQTX_REG &= ~IIS_IRQTX;
     }
 
-    set_fiq_status(status);
+    restore_fiq(status);
 }
 
 void pcm_play_unlock(void)
 {
-   int status = set_fiq_status(FIQ_DISABLED);
+   int status = disable_fiq_save();
 
     if (--dma_play_data.locked == 0 && dma_play_data.state != 0) {
         IIS_IRQTX_REG |= IIS_IRQTX;
     }
 
-   set_fiq_status(status);
+   restore_fiq(status);
 }
 
 static void play_start_pcm(void)
@@ -373,22 +373,22 @@ static struct dma_data dma_rec_data NOCACHEBSS_ATTR =
    will require other measures */
 void pcm_rec_lock(void)
 {
-    int status = set_fiq_status(FIQ_DISABLED);
+    int status = disable_fiq_save();
 
     if (++dma_rec_data.locked == 1)
         IIS_IRQRX_REG &= ~IIS_IRQRX;
 
-    set_fiq_status(status);
+    restore_fiq(status);
 }
 
 void pcm_rec_unlock(void)
 {
-    int status = set_fiq_status(FIQ_DISABLED);
+    int status = disable_fiq_save();
 
     if (--dma_rec_data.locked == 0 && dma_rec_data.state != 0)
         IIS_IRQRX_REG |= IIS_IRQRX;
 
-    set_fiq_status(status);
+    restore_fiq(status);
 }
 
 /* NOTE: direct stack use forbidden by GCC stack handling bug for FIQ */
diff --git a/firmware/target/arm/pnx0101/iriver-ifp7xx/power-ifp7xx.c b/firmware/target/arm/pnx0101/iriver-ifp7xx/power-ifp7xx.c
index b219244510..4494742bb8 100644
--- a/firmware/target/arm/pnx0101/iriver-ifp7xx/power-ifp7xx.c
+++ b/firmware/target/arm/pnx0101/iriver-ifp7xx/power-ifp7xx.c
@@ -55,11 +55,10 @@ bool ide_powered(void)
 
 void power_off(void)
 {
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_interrupt(IRQ_FIQ_STATUS);
     GPIO1_CLR = 1 << 16;
     GPIO2_SET = 1;
-    while(1)
-        yield();
+    while(1);
 }
 
 #else
diff --git a/firmware/target/arm/pnx0101/system-pnx0101.c b/firmware/target/arm/pnx0101/system-pnx0101.c
index da94f397ca..bedcff04ea 100644
--- a/firmware/target/arm/pnx0101/system-pnx0101.c
+++ b/firmware/target/arm/pnx0101/system-pnx0101.c
@@ -113,7 +113,7 @@ static inline void *noncached(void *p)
 static void do_set_mem_timings(void) ICODE_ATTR;
 static void do_set_mem_timings(void)
 {
-    int old_irq = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int old_irq = disable_irq_save();
     while ((EMC.status & 3) != 0);
     EMC.control = 5;
     EMCSTATIC0.waitrd = 6;
@@ -130,7 +130,7 @@ static void do_set_mem_timings(void)
     EMCSTATIC1.config = 0x80081;
 #endif
     EMC.control = 1;
-    set_irq_level(old_irq);
+    restore_irq(old_irq);
 }
 
 static void emc_set_mem_timings(void)
diff --git a/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c b/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
index 7f25cb6a15..00be543bb6 100644
--- a/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
+++ b/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
@@ -66,27 +66,27 @@ static void _pcm_apply_settings(void)
 
 void pcm_apply_settings(void)
 {
-    int oldstatus = set_fiq_status(FIQ_DISABLED);
+    int status = disable_fiq_save();
     _pcm_apply_settings();
-    set_fiq_status(oldstatus);
+    restore_fiq(status);
 }
 
 /* For the locks, DMA interrupt must be disabled because the handler
    manipulates INTMSK and the operation is not atomic */
 void pcm_play_lock(void)
 {
-    int status = set_fiq_status(FIQ_DISABLED);
+    int status = disable_fiq_save();
     if (++dma_play_lock.locked == 1)
         INTMSK |= (1<<19); /* Mask the DMA interrupt */
-    set_fiq_status(status);
+    restore_fiq(status);
 }
 
 void pcm_play_unlock(void)
 {
-    int status = set_fiq_status(FIQ_DISABLED);
+    int status = disable_fiq_save();
     if (--dma_play_lock.locked == 0)
         INTMSK &= ~dma_play_lock.state; /* Unmask the DMA interrupt if enabled */
-    set_fiq_status(status);
+    restore_fiq(status);
 }
 
 void pcm_play_dma_init(void)
diff --git a/firmware/target/arm/s3c2440/gigabeat-fx/timer-meg-fx.c b/firmware/target/arm/s3c2440/gigabeat-fx/timer-meg-fx.c
index 7df20f7149..b59e95806d 100644
--- a/firmware/target/arm/s3c2440/gigabeat-fx/timer-meg-fx.c
+++ b/firmware/target/arm/s3c2440/gigabeat-fx/timer-meg-fx.c
@@ -66,7 +66,7 @@ bool __timer_set(long cycles, bool start)
             pfn_unregister = NULL;
         }
 
-        oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        oldlevel = disable_irq_save();
 
         TCMPB0 = 0;
         TCNTB0 = (unsigned int)cycles / prescaler;
@@ -77,7 +77,7 @@ bool __timer_set(long cycles, bool start)
         TCFG0 = (TCFG0 & ~0xff) | (prescaler - 1);
         TCFG1 = (TCFG1 & ~0xf) | divider;
 
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
 
         retval = true;
     }
diff --git a/firmware/target/arm/system-arm.h b/firmware/target/arm/system-arm.h
index 774cdbcff4..3f1dfb16c8 100644
--- a/firmware/target/arm/system-arm.h
+++ b/firmware/target/arm/system-arm.h
@@ -74,27 +74,7 @@ static inline uint32_t swap_odd_even32(uint32_t value)
     return value;
 }
 
-static inline void enable_fiq(void)
-{
-    /* Clear FIQ disable bit */
-    asm volatile (
-        "mrs     r0, cpsr         \n"\
-        "bic     r0, r0, #0x40    \n"\
-        "msr     cpsr_c, r0         "
-        : : : "r0"
-    );
-}
-
-static inline void disable_fiq(void)
-{
-    /* Set FIQ disable bit */
-    asm volatile (
-        "mrs     r0, cpsr         \n"\
-        "orr     r0, r0, #0x40    \n"\
-        "msr     cpsr_c, r0         "
-        : : : "r0"
-    );
-}
+/* Core-level interrupt masking */
 
 /* This one returns the old status */
 #define IRQ_ENABLED      0x00
@@ -108,8 +88,10 @@ static inline void disable_fiq(void)
 #define IRQ_FIQ_STATUS   0xc0
 #define HIGHEST_IRQ_LEVEL IRQ_DISABLED
 
-#define set_irq_level(status)  set_interrupt_status((status), IRQ_STATUS)
-#define set_fiq_status(status) set_interrupt_status((status), FIQ_STATUS)
+#define set_irq_level(status) \
+    set_interrupt_status((status), IRQ_STATUS)
+#define set_fiq_status(status) \
+    set_interrupt_status((status), FIQ_STATUS)
 
 static inline int set_interrupt_status(int status, int mask)
 {
@@ -122,10 +104,75 @@ static inline int set_interrupt_status(int status, int mask)
         "orr    %0, %0, %2      \n"
         "msr    cpsr_c, %0      \n"
         : "=&r,r"(cpsr), "=&r,r"(oldstatus)
-        : "r,i"(status & mask), [mask]"i,i"(mask)
-    );
+        : "r,i"(status & mask), [mask]"i,i"(mask));
 
     return oldstatus;
 }
 
+static inline void enable_interrupt(int mask)
+{
+    /* Clear I and/or F disable bit */
+    int tmp;
+    asm volatile (
+        "mrs     %0, cpsr   \n"
+        "bic     %0, %0, %1 \n"
+        "msr     cpsr_c, %0 \n"
+        : "=&r"(tmp) : "i"(mask));
+}
+
+static inline void disable_interrupt(int mask)
+{
+    /* Set I and/or F disable bit */
+    int tmp;
+    asm volatile (
+        "mrs     %0, cpsr   \n"
+        "orr     %0, %0, %1 \n"
+        "msr     cpsr_c, %0 \n"
+        : "=&r"(tmp) : "i"(mask));
+}
+
+#define disable_irq(void) \
+    disable_interrupt(IRQ_STATUS)
+
+#define enable_irq(void) \
+    enable_interrupt(IRQ_STATUS)
+
+#define disable_fiq(void) \
+    disable_interrupt(FIQ_STATUS)
+
+#define enable_fiq(void) \
+    enable_interrupt(FIQ_STATUS)
+
+static inline int disable_interrupt_save(int mask)
+{
+    /* Set I and/or F disable bit and return old cpsr value */
+    int cpsr, tmp;
+    asm volatile (
+        "mrs     %1, cpsr   \n"
+        "orr     %0, %1, %2 \n"
+        "msr     cpsr_c, %0 \n"
+        : "=&r"(tmp), "=&r"(cpsr)
+        : "i"(mask));
+    return cpsr;
+}
+
+#define disable_irq_save() \
+    disable_interrupt_save(IRQ_STATUS)
+
+#define disable_fiq_save() \
+    disable_interrupt_save(FIQ_STATUS)
+
+static inline void restore_interrupt(int cpsr)
+{
+    /* Set cpsr_c from value returned by disable_interrupt_save
+     * or set_interrupt_status */
+    asm volatile ("msr cpsr_c, %0" : : "r"(cpsr));
+}
+
+#define restore_irq(cpsr) \
+    restore_interrupt(cpsr)
+
+#define restore_fiq(cpsr) \
+    restore_interrupt(cpsr)
+
 #endif /* SYSTEM_ARM_H */
diff --git a/firmware/target/arm/tatung/tpj1022/backlight-tpj1022.c b/firmware/target/arm/tatung/tpj1022/backlight-tpj1022.c
index 933ad5e930..a466cd93dd 100644
--- a/firmware/target/arm/tatung/tpj1022/backlight-tpj1022.c
+++ b/firmware/target/arm/tatung/tpj1022/backlight-tpj1022.c
@@ -28,17 +28,17 @@
 void _backlight_on(void)
 {
 #if 0
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     pcf50606_write(0x38, 0xb0); /* Backlight ON, GPO1INV=1, GPO1ACT=011 */
-    set_irq_level(level);
+    restore_irq(level);
 #endif
 }
 
 void _backlight_off(void)
 {
 #if 0
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     pcf50606_write(0x38, 0x80); /* Backlight OFF, GPO1INV=1, GPO1ACT=000 */
-    set_irq_level(level);
+    restore_irq(level);
 #endif
 }
diff --git a/firmware/target/arm/tms320dm320/dsp-dm320.c b/firmware/target/arm/tms320dm320/dsp-dm320.c
index f2ca954b24..e082cf7e4d 100644
--- a/firmware/target/arm/tms320dm320/dsp-dm320.c
+++ b/firmware/target/arm/tms320dm320/dsp-dm320.c
@@ -85,7 +85,7 @@ void dsp_wake(void)
 {
     /* If this is called concurrently, we may overlap setting and resetting the
        bit, which causes lost interrupts to the DSP. */
-    int old_level = set_irq_level(IRQ_DISABLED);
+    int old_level = disable_irq_save();
     
     /* The first time you INT0 the DSP, the ROM loader will branch to your RST
        handler. Subsequent times, your INT0 handler will get executed. */
@@ -93,7 +93,7 @@ void dsp_wake(void)
     nop; nop;
     IO_DSPC_HPIB_CONTROL |= 1 << 7;
     
-    set_irq_level(old_level);
+    restore_irq(old_level);
 }
 
 static void dsp_load(const struct dsp_section *im)
diff --git a/firmware/target/arm/tms320dm320/timer-dm320.c b/firmware/target/arm/tms320dm320/timer-dm320.c
index 482fef9f12..e1e051d3a5 100644
--- a/firmware/target/arm/tms320dm320/timer-dm320.c
+++ b/firmware/target/arm/tms320dm320/timer-dm320.c
@@ -48,7 +48,7 @@ bool __timer_set(long cycles, bool start)
         pfn_unregister = NULL;
     }
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
 
     /* Increase prescale values starting from 0 to make the cycle count fit */
     while(divider>65535 && prescaler<1024)
@@ -60,7 +60,7 @@ bool __timer_set(long cycles, bool start)
     IO_TIMER0_TMPRSCL = prescaler;
     IO_TIMER0_TMDIV = divider;
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     return true;
 }
diff --git a/firmware/target/coldfire/iaudio/adc-iaudio.c b/firmware/target/coldfire/iaudio/adc-iaudio.c
index 1895cacfe9..5a2de24692 100644
--- a/firmware/target/coldfire/iaudio/adc-iaudio.c
+++ b/firmware/target/coldfire/iaudio/adc-iaudio.c
@@ -44,7 +44,7 @@ unsigned short adc_scan(int channel)
     int level;
     int data;
 
-    level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    level = disable_irq_save();
 
     pcf50606_write(0x2f, adcc2_parms[channel]);
     data = pcf50606_read(0x30);
@@ -52,7 +52,7 @@ unsigned short adc_scan(int channel)
     if (channel == ADC_BATTERY)
         data = get_10bit_voltage(data);
 
-    set_irq_level(level);
+    restore_irq(level);
 
     return (unsigned short)data;
 }
diff --git a/firmware/target/coldfire/iaudio/m3/audio-m3.c b/firmware/target/coldfire/iaudio/m3/audio-m3.c
index 4edae48744..da8c43c405 100644
--- a/firmware/target/coldfire/iaudio/m3/audio-m3.c
+++ b/firmware/target/coldfire/iaudio/m3/audio-m3.c
@@ -33,7 +33,7 @@ void audio_set_output_source(int source)
         txsrc = (4 << 8); /* recording, iis1RcvData */
 
     IIS1CONFIG = (IIS1CONFIG & ~(7 << 8)) | txsrc;
-    set_irq_level(level);
+    restore_irq(level);
 } /* audio_set_output_source */
 
 void audio_input_mux(int source, unsigned flags)
diff --git a/firmware/target/coldfire/iaudio/m5/audio-m5.c b/firmware/target/coldfire/iaudio/m5/audio-m5.c
index 87a7c35574..024621be64 100644
--- a/firmware/target/coldfire/iaudio/m5/audio-m5.c
+++ b/firmware/target/coldfire/iaudio/m5/audio-m5.c
@@ -32,7 +32,7 @@ void audio_set_output_source(int source)
         txsrc = (4 << 8); /* recording, iis1RcvData */
 
     IIS1CONFIG = (IIS1CONFIG & ~(7 << 8)) | txsrc;
-    set_irq_level(level);
+    restore_irq(level);
 } /* audio_set_output_source */
 
 void audio_input_mux(int source, unsigned flags)
diff --git a/firmware/target/coldfire/iaudio/m5/backlight-m5.c b/firmware/target/coldfire/iaudio/m5/backlight-m5.c
index 19bc5bfcae..3461a330ec 100644
--- a/firmware/target/coldfire/iaudio/m5/backlight-m5.c
+++ b/firmware/target/coldfire/iaudio/m5/backlight-m5.c
@@ -33,18 +33,18 @@ bool _backlight_init(void)
 
 void _backlight_on(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
 
     pcf50606_write(0x39, 0x07);
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 void _backlight_off(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
 
     pcf50606_write(0x39, 0x00);
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 void _remote_backlight_on(void)
diff --git a/firmware/target/coldfire/iaudio/m5/power-m5.c b/firmware/target/coldfire/iaudio/m5/power-m5.c
index 939cae02f2..18401af49f 100644
--- a/firmware/target/coldfire/iaudio/m5/power-m5.c
+++ b/firmware/target/coldfire/iaudio/m5/power-m5.c
@@ -44,16 +44,16 @@ bool charger_inserted(void)
 void ide_power_enable(bool on)
 {
     /* GPOOD3 */
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     pcf50606_write(0x3c, on ? 0x07 : 0x00);
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 bool ide_powered(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     int value = pcf50606_read(0x3c);
-    set_irq_level(level);
+    restore_irq(level);
     return (value & 0x07) != 0;
 }
 
diff --git a/firmware/target/coldfire/iaudio/pcf50606-iaudio.c b/firmware/target/coldfire/iaudio/pcf50606-iaudio.c
index 0614355b5d..14b45e78b5 100644
--- a/firmware/target/coldfire/iaudio/pcf50606-iaudio.c
+++ b/firmware/target/coldfire/iaudio/pcf50606-iaudio.c
@@ -94,9 +94,9 @@ void pcf50606_init(void)
 
 void pcf50606_reset_timeout(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     pcf50606_write(0x08, pcf50606_read(0x08) | 0x02); /* OOCC1 - TOTRST=1 */
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 /* Handles interrupts generated by the pcf50606 */
diff --git a/firmware/target/coldfire/iaudio/x5/audio-x5.c b/firmware/target/coldfire/iaudio/x5/audio-x5.c
index 594ff3b574..0ae5f6023a 100644
--- a/firmware/target/coldfire/iaudio/x5/audio-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/audio-x5.c
@@ -32,7 +32,7 @@ void audio_set_output_source(int source)
         txsrc = (4 << 8); /* recording, iis1RcvData */
 
     IIS1CONFIG = (IIS1CONFIG & ~(7 << 8)) | txsrc;
-    set_irq_level(level);
+    restore_irq(level);
 } /* audio_set_output_source */
 
 void audio_input_mux(int source, unsigned flags)
diff --git a/firmware/target/coldfire/iaudio/x5/backlight-x5.c b/firmware/target/coldfire/iaudio/x5/backlight-x5.c
index ebb28fbf2b..13ccad5adf 100644
--- a/firmware/target/coldfire/iaudio/x5/backlight-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/backlight-x5.c
@@ -39,16 +39,16 @@ void _backlight_on(void)
 #ifndef BOOTLOADER
     _lcd_sleep_timer = 0; /* LCD should be awake already */
 #endif
-    level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    level = disable_irq_save();
     pcf50606_write(0x38, 0xb0); /* Backlight ON, GPO1INV=1, GPO1ACT=011 */
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 void _backlight_off(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     pcf50606_write(0x38, 0x80); /* Backlight OFF, GPO1INV=1, GPO1ACT=000 */
-    set_irq_level(level);
+    restore_irq(level);
     lcd_enable(false);
 #ifndef BOOTLOADER
     /* Start LCD sleep countdown */
@@ -66,10 +66,10 @@ void _backlight_off(void)
 void _backlight_set_brightness(int val)
 {
     /* disable IRQs while bitbanging */
-    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int old_irq_level = disable_irq_save();
     pcf50606_write(0x35, (val << 1) | 0x01); /* 512Hz, Enable PWM */
     /* enable IRQs again */
-    set_irq_level(old_irq_level);
+    restore_irq(old_irq_level);
 }
 
 void _remote_backlight_on(void)
diff --git a/firmware/target/coldfire/iaudio/x5/ds2411-x5.c b/firmware/target/coldfire/iaudio/x5/ds2411-x5.c
index dc5a188ae1..a9bf20f727 100644
--- a/firmware/target/coldfire/iaudio/x5/ds2411-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/ds2411-x5.c
@@ -134,7 +134,7 @@ static unsigned char ds2411_read_byte(void)
  */
 int ds2411_read_id(struct ds2411_id *id)
 {
-    int level = set_irq_level(DISABLE_INTERRUPTS); /* Timing sensitive */
+    int level = disable_irq_save(); /* Timing sensitive */
     int i;
     unsigned char crc;
 
@@ -208,7 +208,7 @@ int ds2411_read_id(struct ds2411_id *id)
         i = DS2411_NO_PRESENCE;
     }
 
-    set_irq_level(level);
+    restore_irq(level);
 
     return i;
 } /* ds2411_read_id */
diff --git a/firmware/target/coldfire/iaudio/x5/power-x5.c b/firmware/target/coldfire/iaudio/x5/power-x5.c
index 068b25f577..f5193aa559 100644
--- a/firmware/target/coldfire/iaudio/x5/power-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/power-x5.c
@@ -44,16 +44,16 @@ bool charger_inserted(void)
 void ide_power_enable(bool on)
 {
     /* GPOOD3 */
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     pcf50606_write(0x3c, on ? 0x07 : 0x00);
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 bool ide_powered(void)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     int value = pcf50606_read(0x3c);
-    set_irq_level(level);
+    restore_irq(level);
     return (value & 0x07) != 0;
 }
 
diff --git a/firmware/target/coldfire/iriver/audio-iriver.c b/firmware/target/coldfire/iriver/audio-iriver.c
index 4d22e7c987..51917b1338 100644
--- a/firmware/target/coldfire/iriver/audio-iriver.c
+++ b/firmware/target/coldfire/iriver/audio-iriver.c
@@ -41,7 +41,7 @@ void audio_set_output_source(int source)
 
     IIS2CONFIG = (IIS2CONFIG & ~(7 << 8)) | (txsrc_select[source+1] << 8);
 
-    set_irq_level(level);
+    restore_irq(level);
 } /* audio_set_output_source */
 
 void audio_input_mux(int source, unsigned flags)
diff --git a/firmware/target/coldfire/iriver/h100/adc-h100.c b/firmware/target/coldfire/iriver/h100/adc-h100.c
index 40fcc8925d..7ea7618b09 100644
--- a/firmware/target/coldfire/iriver/h100/adc-h100.c
+++ b/firmware/target/coldfire/iriver/h100/adc-h100.c
@@ -47,7 +47,7 @@
 
 unsigned short adc_scan(int channel)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     unsigned char data = 0;
     int i;
     
@@ -97,7 +97,7 @@ unsigned short adc_scan(int channel)
 
     CS_HI;
 
-    set_irq_level(level);
+    restore_irq(level);
     return data;
 }
 
diff --git a/firmware/target/coldfire/iriver/h100/power-h100.c b/firmware/target/coldfire/iriver/h100/power-h100.c
index 5dc8618cc6..7ecf961842 100644
--- a/firmware/target/coldfire/iriver/h100/power-h100.c
+++ b/firmware/target/coldfire/iriver/h100/power-h100.c
@@ -115,8 +115,7 @@ void power_off(void)
     set_irq_level(DISABLE_INTERRUPTS);
     and_l(~0x00080000, &GPIO1_OUT);
     asm("halt");
-    while(1)
-        yield();
+    while(1);
 }
 
 #endif /* SIMULATOR */
diff --git a/firmware/target/coldfire/iriver/h100/spdif-h100.c b/firmware/target/coldfire/iriver/h100/spdif-h100.c
index beede3fdce..380622422c 100644
--- a/firmware/target/coldfire/iriver/h100/spdif-h100.c
+++ b/firmware/target/coldfire/iriver/h100/spdif-h100.c
@@ -99,7 +99,7 @@ void spdif_set_output_source(int source, bool src_on)
         PDOR3 = 0; /* A write to the FIFO kick-starts playback */
     }
 
-    set_irq_level(level);
+    restore_irq(level);
 } /* spdif_set_output_source */
 
 /* Return the last set S/PDIF audio source */
diff --git a/firmware/target/coldfire/iriver/h300/adc-h300.c b/firmware/target/coldfire/iriver/h300/adc-h300.c
index b13d0c73cc..efe5a9d746 100644
--- a/firmware/target/coldfire/iriver/h300/adc-h300.c
+++ b/firmware/target/coldfire/iriver/h300/adc-h300.c
@@ -34,7 +34,7 @@ static int adcc2_parms[] =
 
 unsigned short adc_scan(int channel)
 {
-    int level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int level = disable_irq_save();
     unsigned data;
 
     pcf50606_write(0x2f, adcc2_parms[channel]);
@@ -43,6 +43,6 @@ unsigned short adc_scan(int channel)
     if (channel == ADC_BATTERY)
         data = (data << 2) | (pcf50606_read(0x31) & 0x03);
 
-    set_irq_level(level);
+    restore_irq(level);
     return data;
 }
diff --git a/firmware/target/coldfire/iriver/h300/backlight-h300.c b/firmware/target/coldfire/iriver/h300/backlight-h300.c
index c0d441ad3f..f042ee5524 100644
--- a/firmware/target/coldfire/iriver/h300/backlight-h300.c
+++ b/firmware/target/coldfire/iriver/h300/backlight-h300.c
@@ -51,10 +51,10 @@ void _backlight_off(void)
 void _backlight_set_brightness(int val)
 {
     /* disable IRQs while bitbanging */
-    int old_irq_level = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int old_irq_level = disable_irq_save();
     pcf50606_write(0x35, (val << 1) | 0x01); /* 512Hz, Enable PWM */
     /* enable IRQs again */
-    set_irq_level(old_irq_level);
+    restore_irq(old_irq_level);
 }
 
 void _remote_backlight_on(void)
diff --git a/firmware/target/coldfire/iriver/h300/power-h300.c b/firmware/target/coldfire/iriver/h300/power-h300.c
index 21c723da8e..a06763e555 100644
--- a/firmware/target/coldfire/iriver/h300/power-h300.c
+++ b/firmware/target/coldfire/iriver/h300/power-h300.c
@@ -86,8 +86,7 @@ void power_off(void)
     set_irq_level(DISABLE_INTERRUPTS);
     and_l(~0x00080000, &GPIO1_OUT);
     asm("halt");
-    while(1)
-        yield();
+    while(1);
 }
 
 #endif /* SIMULATOR */
diff --git a/firmware/target/coldfire/iriver/lcd-remote-iriver.c b/firmware/target/coldfire/iriver/lcd-remote-iriver.c
index cab7cc4104..2fe5d6c3c1 100644
--- a/firmware/target/coldfire/iriver/lcd-remote-iriver.c
+++ b/firmware/target/coldfire/iriver/lcd-remote-iriver.c
@@ -478,9 +478,9 @@ static void remote_tick(void)
             if (!(countdown % 8))
             {
                 /* Determine which type of remote it is */
-                level = set_irq_level(HIGHEST_IRQ_LEVEL);
+                level = disable_irq_save();
                 val = adc_scan(ADC_REMOTEDETECT);
-                set_irq_level(level);
+                restore_irq(level);
 
                 if (val < ADCVAL_H100_LCD_REMOTE_HOLD)
                 {
diff --git a/firmware/target/coldfire/pcm-coldfire.c b/firmware/target/coldfire/pcm-coldfire.c
index 105ad51de6..2614946543 100644
--- a/firmware/target/coldfire/pcm-coldfire.c
+++ b/firmware/target/coldfire/pcm-coldfire.c
@@ -158,7 +158,7 @@ void _pcm_apply_settings_irq_lock(bool clear_reset)
 {
     int level = set_irq_level(DMA_IRQ_LEVEL);
     _pcm_apply_settings(clear_reset);
-    set_irq_level(level);
+    restore_irq(level);
 }
 
 /* This clears the reset bit to enable monitoring immediately if monitoring
@@ -175,7 +175,7 @@ void pcm_apply_settings(void)
     if (_pcm_apply_settings(!pbm || kick) && kick)
         PDOR3 = 0; /* Kick FIFO out of reset by writing to it */
 
-    set_irq_level(level);
+    restore_irq(level);
 } /* pcm_apply_settings */
 
 void pcm_play_dma_init(void)
diff --git a/firmware/target/coldfire/system-coldfire.c b/firmware/target/coldfire/system-coldfire.c
index 60f7ab12ad..67c684f95a 100644
--- a/firmware/target/coldfire/system-coldfire.c
+++ b/firmware/target/coldfire/system-coldfire.c
@@ -351,5 +351,5 @@ void coldfire_set_dataincontrol(unsigned long value)
     /* Have to be atomic against recording stop initiated by DMA1 */
     int level = set_irq_level(DMA_IRQ_LEVEL);
     DATAINCONTROL = (DATAINCONTROL & (1 << 9)) | value;
-    set_irq_level(level);
+    restore_irq(level);
 }
diff --git a/firmware/target/coldfire/system-target.h b/firmware/target/coldfire/system-target.h
index 8e3a2a3d5a..14b3207c0c 100644
--- a/firmware/target/coldfire/system-target.h
+++ b/firmware/target/coldfire/system-target.h
@@ -81,13 +81,54 @@ static inline int set_irq_level(int level)
 {
     int oldlevel;
     /* Read the old level and set the new one */
-    asm volatile ("move.w %%sr, %0 \n"
-                  "bset.l #13, %1  \n" /* Keep supervisor state set */
-                  "move.w %1, %%sr \n"
-                  : "=d"(oldlevel), "+d"(level));
+
+    /* Not volatile - can be removed if oldlevel isn't used */
+    asm          ("move.w %%sr, %0" : "=d"(oldlevel));
+    /* Keep supervisor state set */
+    asm volatile ("move.w %0, %%sr \n" : : "d"(level | 0x2000));
+    return oldlevel;
+}
+
+/* Enable all interrupts */
+static inline void enable_irq(void)
+{
+    int tmp;
+    /* Using move.w over the compiler's move.l saves 2 bytes per instance */
+    asm volatile ("move.w %1, %0   \n"
+                  "move.w %0, %%sr \n"
+                  : "=&d"(tmp) : "i"(0x2000));
+}
+
+/* Disable interrupts up to HIGHEST_IRQ_LEVEL */
+static inline void disable_irq(void)
+{
+    int tmp;
+    /* Using move.w over the compiler's move.l saves 2 bytes per instance */
+    asm volatile ("move.w %1, %0   \n"
+                  "move.w %0, %%sr \n"
+                  : "=&d"(tmp)
+                  : "i"(0x2000 | HIGHEST_IRQ_LEVEL));
+}
+
+static inline int disable_irq_save(void)
+{
+    int oldlevel, tmp;
+    /* Using move.w over the compiler's move.l saves 2 bytes per instance */
+    asm volatile ("move.w %%sr, %1 \n"
+                  "move.w %2, %0   \n"
+                  "move.w %0, %%sr \n"
+                  : "=&d"(tmp), "=d"(oldlevel)
+                  : "i"(0x2000 | HIGHEST_IRQ_LEVEL));
     return oldlevel;
 }
 
+static inline void restore_irq(int oldlevel)
+{
+    /* Restore the sr value returned by disable_irq_save or
+     * set_irq_level */
+    asm volatile ("move.w %0, %%sr" : : "d"(oldlevel));
+}
+
 static inline uint16_t swap16(uint16_t value)
   /*
     result[15..8] = value[ 7..0];
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];
diff --git a/firmware/test/i2c/main.c b/firmware/test/i2c/main.c
index 0d54da5dea..e094a0b32e 100644
--- a/firmware/test/i2c/main.c
+++ b/firmware/test/i2c/main.c
@@ -542,7 +542,7 @@ int main(void)
 
 
 
-    set_irq_level(0);
+    enable_irq();
 
 
 
diff --git a/firmware/test/kernel/main.c b/firmware/test/kernel/main.c
index 99642c867d..cc0d93a6b4 100644
--- a/firmware/test/kernel/main.c
+++ b/firmware/test/kernel/main.c
@@ -59,7 +59,7 @@ int main(void)
 
     kernel_init();
 
-    set_irq_level(0);
+    enable_irq();
 
     tick_add_task(testfunc);
     
diff --git a/firmware/thread.c b/firmware/thread.c
index 259a66a652..2ac7f6efc3 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -399,7 +399,7 @@ static inline void core_sleep(void)
 {
     PROC_CTL(CURRENT_CORE) = PROC_SLEEP;
     nop; nop; nop;
-    set_irq_level(IRQ_ENABLED);
+    enable_irq();
 }
 #else
 static inline void core_sleep(unsigned int core)
@@ -421,9 +421,6 @@ static inline void core_sleep(unsigned int core)
         "ldr    r1, [%[mbx], #0]           \n"
         "tst    r1, r0, lsr #2             \n"
         "bne    1b                         \n"
-        "mrs    r1, cpsr                   \n" /* Enable IRQ */
-        "bic    r1, r1, #0x80              \n"
-        "msr    cpsr_c, r1                 \n"
         :
         :  [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), [c]"r"(core)
         : "r0", "r1");
@@ -443,10 +440,8 @@ static inline void core_sleep(unsigned int core)
 
     /* Wait for other processor to finish wake procedure */
     while (MBX_MSG_STAT & (0x1 << core));
-
-    /* Enable IRQ */
-    set_irq_level(IRQ_ENABLED);
 #endif /* ASM/C selection */
+    enable_irq();
 }
 #endif /* NUM_CORES */
 #elif CONFIG_CPU == PP5002
@@ -465,13 +460,11 @@ static inline void core_sleep(void)
         "nop                  \n" /* nop's needed because of pipeline */
         "nop                  \n"
         "nop                  \n"
-        "mrs    r0, cpsr      \n" /* Enable IRQ */
-        "bic    r0, r0, #0x80 \n"
-        "msr    cpsr_c, r0    \n"
         :
         : [ctl]"r"(&PROC_CTL(CURRENT_CORE))
         : "r0"
     );
+    enable_irq();
 }
 #else
 /* PP5002 has no mailboxes - emulate using bytes */
@@ -503,9 +496,6 @@ static inline void core_sleep(unsigned int core)
         "ldrb   r0, [%[sem], #0]           \n"
         "cmp    r0, #0                     \n"
         "bne    1b                         \n"
-        "mrs    r0, cpsr                   \n" /* Enable IRQ */
-        "bic    r0, r0, #0x80              \n"
-        "msr    cpsr_c, r0                 \n"
         :
         : [sem]"r"(&core_semaphores[core]), [c]"r"(core),
           [ctl]"r"(&PROC_CTL(CPU))
@@ -530,8 +520,8 @@ static inline void core_sleep(unsigned int core)
     while (core_semaphores[core].intend_wake != 0);
 
     /* Enable IRQ */
-    set_irq_level(IRQ_ENABLED);
 #endif /* ASM/C selection */
+    enable_irq();
 }
 #endif /* NUM_CORES */
 #endif /* PP CPU type */
@@ -578,7 +568,7 @@ void core_wake(unsigned int othercore)
         : "r1", "r2", "r3");
 #else /* C version for reference */
     /* Disable interrupts - avoid reentrancy from the tick */
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     /* Signal intent to wake other processor - set stay awake */
     MBX_MSG_SET = 0x11 << othercore;
@@ -593,7 +583,7 @@ void core_wake(unsigned int othercore)
 
     /* Done with wake procedure */
     MBX_MSG_CLR = 0x1 << othercore;
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 #endif /* ASM/C selection */
 }
 #elif CONFIG_CPU == PP5002
@@ -631,7 +621,7 @@ void core_wake(unsigned int othercore)
     );
 #else /* C version for reference */
     /* Disable interrupts - avoid reentrancy from the tick */
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     /* Signal intent to wake other processor - set stay awake */
     core_semaphores[othercore].intend_wake = 1;
@@ -647,7 +637,7 @@ void core_wake(unsigned int othercore)
 
     /* Done with wake procedure */
     core_semaphores[othercore].intend_wake = 0;
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 #endif  /* ASM/C selection */
 }
 #endif /* CPU type */
@@ -775,7 +765,7 @@ static inline void core_sleep(void)
 static inline void core_sleep(void)
 {
     #warning TODO: Implement core_sleep
-    set_irq_level(IRQ_ENABLED);
+    enable_irq();
 }
 #elif defined(CPU_TCC780X)
 static inline void core_sleep(void)
@@ -784,11 +774,9 @@ static inline void core_sleep(void)
     asm volatile (
         "mov r0, #0                \n"
         "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
-        "mrs r0, cpsr              \n" /* Unmask IRQ at core level */
-        "bic r0, r0, #0x80         \n"
-        "msr cpsr_c, r0            \n"
         : : : "r0"
     );
+    enable_irq();
 }
 #elif CONFIG_CPU == IMX31L
 static inline void core_sleep(void)
@@ -796,17 +784,15 @@ static inline void core_sleep(void)
     asm volatile (
         "mov r0, #0                \n"
         "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
-        "mrs r0, cpsr              \n" /* Unmask IRQ at core level */
-        "bic r0, r0, #0x80         \n"
-        "msr cpsr_c, r0            \n"
         : : : "r0"
     );
+    enable_irq();
 }
 #else
 static inline void core_sleep(void)
 {
     #warning core_sleep not implemented, battery life will be decreased
-    set_irq_level(0);
+    enable_irq();
 }
 #endif /* CONFIG_CPU == */
 
@@ -1706,14 +1692,14 @@ void check_tmo_threads(void)
     while (next != NULL)
     {
         /* Check sleeping threads. Allow interrupts between checks. */
-        set_irq_level(0);
+        enable_irq();
 
         struct thread_entry *curr = next;
 
         next = curr->tmo.next;
 
         /* Lock thread slot against explicit wakeup */
-        set_irq_level(HIGHEST_IRQ_LEVEL);
+        disable_irq();
         LOCK_THREAD(curr);
 
         unsigned state = curr->state;
@@ -1956,7 +1942,7 @@ void switch_thread(void)
             check_tmo_threads();
         }
 
-        set_irq_level(HIGHEST_IRQ_LEVEL);
+        disable_irq();
         RTR_LOCK(core);
 
         thread = cores[core].running;
@@ -2018,7 +2004,7 @@ void switch_thread(void)
 #endif /* HAVE_PRIORITY_SCHEDULING */
 
             RTR_UNLOCK(core);
-            set_irq_level(0);
+            enable_irq();
             break;
         }
     }
@@ -2212,7 +2198,7 @@ unsigned int thread_queue_wake(struct thread_entry **list)
 static struct thread_entry * find_empty_thread_slot(void)
 {
     /* Any slot could be on an interrupt-accessible list */
-    IF_COP( int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); )
+    IF_COP( int oldlevel = disable_irq_save(); )
     struct thread_entry *thread = NULL;
     int n;
 
@@ -2233,7 +2219,7 @@ static struct thread_entry * find_empty_thread_slot(void)
         UNLOCK_THREAD(t);
     }
 
-    IF_COP( set_irq_level(oldlevel); ) /* Reenable interrups - this slot is
+    IF_COP( restore_irq(oldlevel); ) /* Reenable interrups - this slot is
                                           not accesible to them yet */
     return thread;
 }
@@ -2247,7 +2233,7 @@ static struct thread_entry * find_empty_thread_slot(void)
 void core_idle(void)
 {
     IF_COP( const unsigned int core = CURRENT_CORE; )
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
     core_sleep(IF_COP(core));
 }
 
@@ -2277,7 +2263,7 @@ struct thread_entry*
         return NULL;
     }
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
 
     /* Munge the stack to make it easy to spot stack overflows */
     stackptr = ALIGN_UP((uintptr_t)stack, sizeof (uintptr_t));
@@ -2338,7 +2324,7 @@ struct thread_entry*
 
     UNLOCK_THREAD(thread);
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     return thread;
 }
@@ -2394,7 +2380,7 @@ void thread_wait(struct thread_entry *thread)
         IF_COP( current->obj_cl = &thread->waiter_cl; )
         current->bqp = &thread->queue;
 
-        set_irq_level(HIGHEST_IRQ_LEVEL);
+        disable_irq();
         block_thread(current);
 
         corelock_unlock(&thread->waiter_cl);
@@ -2418,7 +2404,7 @@ void thread_exit(void)
     /* Cancel CPU boost if any */
     cancel_cpu_boost();
 
-    set_irq_level(HIGHEST_IRQ_LEVEL);
+    disable_irq();
 
     corelock_lock(&current->waiter_cl);
     LOCK_THREAD(current);
@@ -2503,7 +2489,7 @@ void remove_thread(struct thread_entry *thread)
     if (thread == current)
         thread_exit(); /* Current thread - do normal exit */
 
-    oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    oldlevel = disable_irq_save();
 
     corelock_lock(&thread->waiter_cl);
     LOCK_THREAD(thread);
@@ -2521,7 +2507,7 @@ void remove_thread(struct thread_entry *thread)
         /* Thread being killed - become a waiter */
         UNLOCK_THREAD(thread);
         corelock_unlock(&thread->waiter_cl);
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
         thread_wait(thread);
         return;
     }
@@ -2543,11 +2529,11 @@ void remove_thread(struct thread_entry *thread)
         corelock_unlock(&thread->waiter_cl);
 
         UNLOCK_THREAD(thread);
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
 
         old_core = switch_core(new_core);
 
-        oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+        oldlevel = disable_irq_save();
 
         corelock_lock(&thread->waiter_cl);
         LOCK_THREAD(thread);
@@ -2643,7 +2629,7 @@ thread_killed: /* Thread was already killed */
     /* Removal complete - safe to unlock and reenable interrupts */
     corelock_unlock(&thread->waiter_cl);
     UNLOCK_THREAD(thread);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
 #if NUM_CORES > 1
     if (old_core < NUM_CORES)
@@ -2675,7 +2661,7 @@ int thread_set_priority(struct thread_entry *thread, int priority)
 
     /* Thread could be on any list and therefore on an interrupt accessible
        one - disable interrupts */
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
 
     LOCK_THREAD(thread);
 
@@ -2788,7 +2774,7 @@ int thread_set_priority(struct thread_entry *thread, int priority)
 
     UNLOCK_THREAD(thread);
 
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 
     return old_base_priority;
 }
@@ -2815,14 +2801,14 @@ int thread_get_priority(struct thread_entry *thread)
  */
 void thread_thaw(struct thread_entry *thread)
 {
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     LOCK_THREAD(thread);
 
     if (thread->state == STATE_FROZEN)
         core_schedule_wakeup(thread);
 
     UNLOCK_THREAD(thread);
-    set_irq_level(oldlevel);
+    restore_irq(oldlevel);
 }
 
 /*---------------------------------------------------------------------------
@@ -2850,14 +2836,14 @@ unsigned int switch_core(unsigned int new_core)
         return core;
     }
 
-    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    int oldlevel = disable_irq_save();
     LOCK_THREAD(current);
 
     if (current->name == THREAD_DESTRUCT)
     {
         /* Thread being killed - deactivate and let process complete */
         UNLOCK_THREAD(current);
-        set_irq_level(oldlevel);
+        restore_irq(oldlevel);
         thread_wait(current);
         /* Should never be reached */
         THREAD_PANICF("switch_core->D:*R", current);
diff --git a/firmware/usb.c b/firmware/usb.c
index 2971ab7041..c33b62f244 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -568,10 +568,10 @@ bool usb_charging_enable(bool on)
 #ifdef IRIVER_H300_SERIES
     int irqlevel;
     logf("usb_charging_enable(%s)\n", on ? "on" : "off" );
-    irqlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
+    irqlevel = disable_irq_save();
     pcf50606_set_usb_charging(on);
     rc = on;
-    (void)set_irq_level(irqlevel);
+    restore_irq(irqlevel);
 #else
     /* TODO: implement it for other targets... */
     (void)on;