8 files changed, 105 insertions, 48 deletions

diff --git a/firmware/kernel.c b/firmware/kernel.c
index 155205749f..0b39e29126 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -1195,9 +1195,7 @@ int semaphore_wait(struct semaphore *s, int timeout)
  * in 'semaphore_init'. */
 void semaphore_release(struct semaphore *s)
 {
-#if defined(HAVE_PRIORITY_SCHEDULING) && defined(irq_enabled_checkval)
     unsigned int result = THREAD_NONE;
-#endif
     int oldlevel;
 
     oldlevel = disable_irq_save();
@@ -1209,11 +1207,7 @@ void semaphore_release(struct semaphore *s)
         KERNEL_ASSERT(s->count == 0,
             "semaphore_release->threads queued but count=%d!\n", s->count);
         s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */
-#if defined(HAVE_PRIORITY_SCHEDULING) && defined(irq_enabled_checkval)
         result = wakeup_thread(&s->queue);
-#else
-        wakeup_thread(&s->queue);
-#endif
     }
     else
     {
@@ -1228,11 +1222,11 @@ void semaphore_release(struct semaphore *s)
     corelock_unlock(&s->cl);
     restore_irq(oldlevel);
 
-#if defined(HAVE_PRIORITY_SCHEDULING) && defined(irq_enabled_checkval)
-    /* No thread switch if IRQ disabled - it's probably called via ISR.
-     * switch_thread would as well enable them anyway. */
-    if((result & THREAD_SWITCH) && irq_enabled_checkval(oldlevel))
+#if defined(HAVE_PRIORITY_SCHEDULING) && defined(is_thread_context)
+    /* No thread switch if not thread context */
+    if((result & THREAD_SWITCH) && is_thread_context())
         switch_thread();
 #endif
+    (void)result;
 }
 #endif /* HAVE_SEMAPHORE_OBJECTS */
diff --git a/firmware/target/arm/as3525/fmradio-i2c-as3525.c b/firmware/target/arm/as3525/fmradio-i2c-as3525.c
index 3da42e31b2..5d05956799 100644
--- a/firmware/target/arm/as3525/fmradio-i2c-as3525.c
+++ b/firmware/target/arm/as3525/fmradio-i2c-as3525.c
@@ -195,8 +195,8 @@ void tuner_isr(void)
     /* read and clear the interrupt */
     if (GPIOA_MIS & (1<<4)) {
         semaphore_release(&rds_sema);
+        GPIOA_IC = (1<<4);
     }
-    GPIOA_IC = (1<<4);
 }
 
 /* Captures RDS data and processes it */
diff --git a/firmware/target/arm/as3525/sansa-fuzev2/button-fuzev2.c b/firmware/target/arm/as3525/sansa-fuzev2/button-fuzev2.c
index 7f82b692c7..8244c475fa 100644
--- a/firmware/target/arm/as3525/sansa-fuzev2/button-fuzev2.c
+++ b/firmware/target/arm/as3525/sansa-fuzev2/button-fuzev2.c
@@ -30,6 +30,7 @@ static bool hold_button = false;
 
 #ifdef HAVE_SCROLLWHEEL
 #define SCROLLWHEEL_BITS        (1<<7|1<<6)
+#define SCROLLWHEEL_BITS_POS    6
                                                       /* TIMER units */
 #define TIMER_TICK              (KERNEL_TIMER_FREQ/HZ)/* how long a tick lasts */
 #define TIMER_MS                (TIMER_TICK/(1000/HZ))/* how long a ms lasts */
@@ -78,10 +79,17 @@ static void scrollwheel(unsigned int wheel_value)
 
     unsigned int btn = BUTTON_NONE;
 
-    if (old_wheel_value == wheel_tbl[0][wheel_value])
+    if (hold_button)
+    {
+    }
+    else if (old_wheel_value == wheel_tbl[0][wheel_value])
+    {
         btn = BUTTON_SCROLL_FWD;
+    }
     else if (old_wheel_value == wheel_tbl[1][wheel_value])
+    {
         btn = BUTTON_SCROLL_BACK;
+    }
 
     if (btn == BUTTON_NONE)
     {
@@ -200,11 +208,13 @@ void button_gpioa_isr(void)
 {
 #if defined(HAVE_SCROLLWHEEL)
     /* scroll wheel handling */
-    if (GPIOA_MIS & SCROLLWHEEL_BITS)
-        scrollwheel(GPIOA_PIN_MASK(0xc0) >> 6);
+    unsigned long bits = GPIOA_MIS & SCROLLWHEEL_BITS;
 
-    /* ack interrupt */
-    GPIOA_IC = SCROLLWHEEL_BITS;
+    if (bits)
+    {
+        scrollwheel(GPIOA_PIN_MASK(SCROLLWHEEL_BITS) >> SCROLLWHEEL_BITS_POS);
+        GPIOA_IC = bits; /* ack interrupt */
+    }
 #endif
 }
 
@@ -225,8 +235,10 @@ int button_read_device(void)
     int delay = 30;
     while(delay--) nop;
 
+    disable_irq();
+
     bool ccu_io_bit12 = CCU_IO & (1<<12);
-    bitclr32(&CCU_IO, 1<<12);
+    CCU_IO &= ~(1<<12);
 
     /* B1 is shared with FM i2c */
     bool gpiob_pin0_dir = GPIOB_DIR & (1<<1);
@@ -256,7 +268,9 @@ int button_read_device(void)
         GPIOB_DIR |= 1<<1;
 
     if(ccu_io_bit12)
-        bitset32(&CCU_IO, 1<<12);
+        CCU_IO |= (1<<12);
+
+    enable_irq();
 
 #ifdef HAS_BUTTON_HOLD
 #ifndef BOOTLOADER
@@ -265,12 +279,6 @@ int button_read_device(void)
     {
         hold_button = hold;
         backlight_hold_changed(hold);
-        /* mask scrollwheel irq so we don't need to check for
-         * the hold button in the isr */
-        if (hold)
-            GPIOA_IE &= ~SCROLLWHEEL_BITS;
-        else
-            GPIOA_IE |= SCROLLWHEEL_BITS;
     }
 #else
     hold_button = hold;
diff --git a/firmware/target/arm/as3525/sd-as3525.c b/firmware/target/arm/as3525/sd-as3525.c
index 17f1bfa11d..5bed36e51e 100644
--- a/firmware/target/arm/as3525/sd-as3525.c
+++ b/firmware/target/arm/as3525/sd-as3525.c
@@ -178,10 +178,12 @@ static int sd1_oneshot_callback(struct timeout *tmo)
 void sd_gpioa_isr(void)
 {
     static struct timeout sd1_oneshot;
+
     if (GPIOA_MIS & EXT_SD_BITS)
+    {
         timeout_register(&sd1_oneshot, sd1_oneshot_callback, (3*HZ/10), 0);
-    /* acknowledge interrupt */
-    GPIOA_IC = EXT_SD_BITS;
+        GPIOA_IC = EXT_SD_BITS; /* acknowledge interrupt */
+    }
 }
 #endif  /* HAVE_HOTSWAP */
 
diff --git a/firmware/target/arm/as3525/sd-as3525v2.c b/firmware/target/arm/as3525/sd-as3525v2.c
index 717d5dd744..9edc598edf 100644
--- a/firmware/target/arm/as3525/sd-as3525v2.c
+++ b/firmware/target/arm/as3525/sd-as3525v2.c
@@ -1041,10 +1041,12 @@ static int sd1_oneshot_callback(struct timeout *tmo)
 void sd_gpioa_isr(void)
 {
     static struct timeout sd1_oneshot;
+
     if (GPIOA_MIS & EXT_SD_BITS)
+    {
         timeout_register(&sd1_oneshot, sd1_oneshot_callback, (3*HZ/10), 0);
-    /* acknowledge interrupt */
-    GPIOA_IC = EXT_SD_BITS;
+        GPIOA_IC = EXT_SD_BITS; /* acknowledge interrupt */
+    }
 }
 #endif /* HAVE_HOTSWAP */
 
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 01aaeaccbe..f0ed75d2bd 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -51,7 +51,7 @@
   extern __attribute__((weak,alias("UIRQ"))) void name (void)
 
 static void UIRQ (void) __attribute__((interrupt ("IRQ")));
-void irq_handler(void) __attribute__((interrupt ("IRQ")));
+void irq_handler(void) __attribute__((naked, interrupt ("IRQ")));
 void fiq_handler(void) __attribute__((interrupt ("FIQ")));
 
 default_interrupt(INT_WATCHDOG);
@@ -121,6 +121,11 @@ static void UIRQ(void)
 static const struct { int source; void (*isr) (void); } vec_int_srcs[] =
 {
     /* Highest priority at the top of the list */
+#if defined(HAVE_HOTSWAP) || defined(HAVE_RDS_CAP) || \
+    (defined(SANSA_FUZEV2) && !INCREASED_SCROLLWHEEL_POLLING)
+    /* If GPIOA ISR is interrupted, things seem to go wonky ?? */
+    { INT_SRC_GPIOA, INT_GPIOA },
+#endif
 #ifdef HAVE_RECORDING
     { INT_SRC_I2SIN, INT_I2SIN }, /* For recording */
 #endif
@@ -134,29 +139,26 @@ static const struct { int source; void (*isr) (void); } vec_int_srcs[] =
     { INT_SRC_TIMER2, INT_TIMER2 },
     { INT_SRC_I2C_AUDIO, INT_I2C_AUDIO },
     { INT_SRC_AUDIO, INT_AUDIO },
-#if defined(HAVE_HOTSWAP) || \
-    (defined(SANSA_FUZEV2) && !INCREASED_SCROLLWHEEL_POLLING)
-    { INT_SRC_GPIOA, INT_GPIOA, },
-#endif
     /* Lowest priority at the end of the list */
 };
 
 static void setup_vic(void)
 {
-    const unsigned int n = sizeof(vec_int_srcs)/sizeof(vec_int_srcs[0]);
-    unsigned int i;
-
     CGU_PERI |= CGU_VIC_CLOCK_ENABLE; /* enable VIC */
     VIC_INT_EN_CLEAR = 0xffffffff; /* disable all interrupt lines */
     VIC_INT_SELECT = 0; /* only IRQ, no FIQ */
 
     *VIC_DEF_VECT_ADDR = UIRQ;
 
-    for(i = 0; i < n; i++)
+    for(unsigned int i = 0; i < ARRAYLEN(vec_int_srcs); i++)
     {
         VIC_VECT_ADDRS[i] = vec_int_srcs[i].isr;
         VIC_VECT_CNTLS[i] = (1<<5) | vec_int_srcs[i].source;
     }
+
+    /* Reset priority hardware */
+    for(unsigned int i = 0; i < 32; i++)
+        *VIC_VECT_ADDR = 0;
 }
 
 void INT_GPIOA(void)
@@ -177,8 +179,36 @@ void INT_GPIOA(void)
 
 void irq_handler(void)
 {
-    (*VIC_VECT_ADDR)(); /* call the isr */
-    *VIC_VECT_ADDR = (void*)VIC_VECT_ADDR; /* any write will ack the irq */
+    /* Worst-case IRQ stack usage with 10 vectors:
+     * 10*4*10 = 400 bytes (100 words)
+     *
+     * No SVC stack is used by pro/epi-logue code
+     */
+    asm volatile (
+        "sub    lr, lr, #4               \n" /* Create return address */
+        "stmfd  sp!, { r0-r5, r12, lr }  \n" /* Save what gets clobbered */
+        "ldr    r0, =0xc6010030          \n" /* Obtain VIC address (before SPSR read!) */
+        "ldr    r12, [r0]                \n" /* Load Vector */
+        "mrs    r1, spsr                 \n" /* Save SPSR_irq */
+        "stmfd  sp!, { r0-r1 }           \n" /* Must have something bet. mrs and msr */
+        "msr    cpsr_c, #0x13            \n" /* Switch to SVC mode, enable IRQ */
+        "and    r4, sp, #4               \n" /* Align SVC stack to 8 bytes, save */
+        "sub    sp, sp, r4               \n"
+        "mov    r5, lr                   \n" /* Save lr_SVC */
+#if ARM_ARCH >= 5
+        "blx    r12                      \n" /* Call handler */
+#else
+        "mov    lr, pc                   \n"
+        "bx     r12                      \n"
+#endif
+        "add    sp, sp, r4               \n" /* Undo alignment fudge */
+        "mov    lr, r5                   \n" /* Restore lr_SVC */
+        "msr    cpsr_c, #0x92            \n" /* Mask IRQ, return to IRQ mode */
+        "ldmfd  sp!, { r0-r1 }           \n" /* Pop VIC address, SPSR_irq */
+        "str    r0, [r0]                 \n" /* Ack end of ISR to VIC  */
+        "msr    spsr_cxsf, r1            \n" /* Restore SPSR_irq */
+        "ldmfd  sp!, { r0-r5, r12, pc }^ \n" /* Restore regs, and RFE */
+    );
 }
 
 void fiq_handler(void)
diff --git a/firmware/target/arm/crt0.S b/firmware/target/arm/crt0.S
index f75f37006d..7befecb593 100644
--- a/firmware/target/arm/crt0.S
+++ b/firmware/target/arm/crt0.S
@@ -100,17 +100,23 @@ newstart:
     strhi   r4, [r2], #4
     bhi     1b
 
-    /* Set up stack for IRQ mode */ 
+    /* Set up stack for IRQ mode */
     msr     cpsr_c, #0xd2
     ldr     sp, =irq_stack
 
-    /* Set up stack for FIQ mode */ 
-    msr     cpsr_c, #0xd1
-    ldr     sp, =fiq_stack
-
-    /* Let svc, abort and undefined modes use irq stack */
     msr     cpsr_c, #0xd3
+#if CONFIG_CPU == AS3525 || CONFIG_CPU == AS3525v2
+    /* Let abort and undefined modes use irq stack */
+    /* svc stack is for interrupt processing */
+    ldr     sp, =svc_stack
+#else
+    /* Let svc, abort and undefined modes use irq stack */
     ldr     sp, =irq_stack
+
+    /* Set up stack for FIQ mode */
+    msr     cpsr_c, #0xd1
+    ldr     sp, =fiq_stack
+#endif
     msr     cpsr_c, #0xd7
     ldr     sp, =irq_stack
     msr     cpsr_c, #0xdb
@@ -159,15 +165,20 @@ prefetch_abort_handler:
     b      UIE
 
 data_abort_handler:
-    sub    r0, lr, #8 
+    sub    r0, lr, #8
     mov    r1, #2
     b      UIE
 
+/* Cache-align interrupt stacks */
+.balign    32
+
 /* 256 words of IRQ stack */
     .space 256*4
 irq_stack:
 
-/* 256 words of FIQ stack */
+/* 256 words of FIQ/SVC stack */
     .space 256*4
 fiq_stack:
-end:
+svc_stack:
+
+end:
\ No newline at end of file
diff --git a/firmware/target/arm/system-arm.h b/firmware/target/arm/system-arm.h
index b3630a8473..ffce77a176 100644
--- a/firmware/target/arm/system-arm.h
+++ b/firmware/target/arm/system-arm.h
@@ -76,6 +76,9 @@ void __div0(void);
 #define ints_enabled_checkval(val) \
     (((val) & IRQ_FIQ_STATUS) == 0)
 
+/* We run in SYS mode */
+#define is_thread_context() \
+    (get_processor_mode() == 0x1f)
 
 /* Core-level interrupt masking */
 
@@ -109,6 +112,13 @@ static inline bool interrupt_enabled(int status)
     return (cpsr & status) == 0;
 }
 
+static inline unsigned long get_processor_mode(void)
+{
+    unsigned long cpsr;
+    asm ("mrs %0, cpsr" : "=r"(cpsr));
+    return cpsr & 0x1f;
+}
+
 /* ARM_ARCH version section for architecture*/
 
 #if ARM_ARCH >= 6