12 files changed, 115 insertions, 69 deletions

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;
 }