5 files changed, 61 insertions, 29 deletions

diff --git a/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c b/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c
index fa74d3bb9f..b04b22911a 100644
--- a/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c
@@ -84,18 +84,24 @@ void __attribute__((naked)) fiq_handler(void)
 
 void avic_init(void)
 {
+    int i;
+
     /* Disable all interrupts and set to unhandled */
     avic_disable_int(ALL);
 
+    /* Reset AVIC control */
+    INTCNTL = 0;
+
     /* Init all interrupts to type IRQ */
     avic_set_int_type(ALL, IRQ);
 
+    /* Set all normal to lowest priority */
+    for (i = 0; i < 8; i++)
+        NIPRIORITY(i) = 0;
+
     /* Set NM bit to enable VIC */
     INTCNTL |= INTCNTL_NM;
 
-    /* Enable IRQ/FIQ in imx31 INTCNTL reg */
-    INTCNTL &= ~(INTCNTL_ABFEN | INTCNTL_NIDIS | INTCNTL_FIDIS);
-
     /* Enable VE bit in CP15 Control reg to enable VIC */
     asm volatile (
         "mrc p15, 0, r0, c1, c0, 0 \n"
@@ -104,11 +110,20 @@ void avic_init(void)
         : : : "r0");
 
     /* Enable normal interrupts at all priorities */
-    NIMASK = 16;
+    NIMASK = 0x1f;
+}
+
+void avic_set_int_priority(enum IMX31_INT_LIST ints,
+                           unsigned long ni_priority)
+{
+    volatile unsigned long *reg = &NIPRIORITY((63 - ints) / 8);
+    unsigned int shift = 4*(ints % 8);
+    unsigned long mask = 0xful << shift;
+    *reg = (*reg & ~mask) | ((ni_priority << shift) & mask);
 }
 
 void avic_enable_int(enum IMX31_INT_LIST ints, enum INT_TYPE intstype,
-                     void (*handler)(void))
+                     unsigned long ni_priority, void (*handler)(void))
 {
     int oldstatus = set_interrupt_status(IRQ_FIQ_DISABLED,
                                          IRQ_FIQ_STATUS);
@@ -118,6 +133,7 @@ void avic_enable_int(enum IMX31_INT_LIST ints, enum INT_TYPE intstype,
         avic_set_int_type(ints, intstype);
         VECTOR(ints) = (long)handler;
         INTENNUM = ints;
+        avic_set_int_priority(ints, ni_priority);
     }
 
     set_interrupt_status(oldstatus, IRQ_FIQ_STATUS);
diff --git a/firmware/target/arm/imx31/gigabeat-s/avic-imx31.h b/firmware/target/arm/imx31/gigabeat-s/avic-imx31.h
index 29a3ec8dd0..7bb7c09da7 100644
--- a/firmware/target/arm/imx31/gigabeat-s/avic-imx31.h
+++ b/firmware/target/arm/imx31/gigabeat-s/avic-imx31.h
@@ -49,7 +49,9 @@ enum IMX31_INT_LIST
 
 void avic_init(void);
 void avic_enable_int(enum IMX31_INT_LIST ints, enum INT_TYPE intstype,
-                     void (*handler)(void));
+                     unsigned long ni_priority, void (*handler)(void));
+void avic_set_int_priority(enum IMX31_INT_LIST ints,
+                           unsigned long ni_priority);
 void avic_disable_int(enum IMX31_INT_LIST ints);
 void avic_set_int_type(enum IMX31_INT_LIST ints, enum INT_TYPE intstype);
 #endif
diff --git a/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c b/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c
index 3c44a156b0..c2fddc40a7 100644
--- a/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c
@@ -7,7 +7,6 @@
 
 extern void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
 
-#ifndef BOOTLOADER
 static __attribute__((interrupt("IRQ"))) void EPIT1_HANDLER(void)
 {
     int i;
@@ -23,14 +22,11 @@ static __attribute__((interrupt("IRQ"))) void EPIT1_HANDLER(void)
 
     current_tick++;
 }
-#endif
 
 void tick_start(unsigned int interval_in_ms)
 {
-    EPITCR1 &= ~(1 << 0);        /* Disable the counter */
-    EPITCR1 |= (1 << 16);        /* Reset */
-
-    CLKCTL_CGR0 |= (0x3 << 6);   /* Clock ON */
+    CLKCTL_CGR0 |= (3 << 6);   /* EPIT1 module clock ON - before writing regs! */
+    EPITCR1 &= ~((1 << 2) | (1 << 0)); /* Disable the counter */
     CLKCTL_WIMR0 &= ~(1 << 23);  /* Clear wakeup mask */
 
     /* NOTE: This isn't really accurate yet but it's close enough to work
@@ -39,17 +35,21 @@ void tick_start(unsigned int interval_in_ms)
     /* CLKSRC=32KHz, EPIT Output Disconnected, Enabled
      * prescale 1/32, Reload from modulus register, Compare interrupt enabled,
      * Count from load value */
-    EPITCR1 = (0x3 << 24) | (1 << 19) | (32 << 4) |
+    EPITCR1 = (3 << 24) | (1 << 19) | (32 << 4) |
               (1 << 3) | (1 << 2) | (1 << 1);
-#ifndef BOOTLOADER
-    EPITLR1 = interval_in_ms;
-    EPITCMPR1 = 0;              /* Event when counter reaches 0 */
-    avic_enable_int(EPIT1, IRQ, EPIT1_HANDLER);
-#else
-    (void)interval_in_ms;
-#endif
-    EPITSR1 = 1;                /* Clear any pending interrupt after
-                                   enabling the vector */
 
+    EPITLR1 = interval_in_ms;   /* Count down from interval */
+    EPITCMPR1 = 0;              /* Event when counter reaches 0 */
+    EPITSR1 = 1;                /* Clear any pending interrupt */
+    avic_enable_int(EPIT1, IRQ, 7, EPIT1_HANDLER);
     EPITCR1 |= (1 << 0);        /* Enable the counter */
 }
+
+#ifdef BOOTLOADER
+void tick_stop(void)
+{
+    avic_disable_int(EPIT1);    /* Disable insterrupt */
+    EPITCR1 &= ~((1 << 2) | (1 << 0)); /* Disable counter */
+    CLKCTL_CGR0 &= ~(3 << 6);   /* EPIT1 module clock OFF */
+}
+#endif
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
index ed5a26cd6e..c77c923d60 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
@@ -1,6 +1,7 @@
 #include "kernel.h"
 #include "system.h"
 #include "panic.h"
+#include "avic-imx31.h"
 #include "mmu-imx31.h"
 #include "system-target.h"
 #include "lcd.h"
@@ -19,11 +20,20 @@ void system_reboot(void)
 
 void system_init(void)
 {
-#ifndef BOOTLOADER
+    /* MCR WFI enables wait mode */
+    CLKCTL_CCMR &= ~(3 << 14);
     avic_init();
-#endif
 }
 
+#ifdef BOOTLOADER
+void system_prepare_fw_start(void)
+{
+    set_interrupt_status(IRQ_FIQ_DISABLED, IRQ_FIQ_STATUS);
+    avic_disable_int(ALL);
+    tick_stop();
+}
+#endif
+
 inline void dumpregs(void) 
 {
         asm volatile ("mov %0,r0\n\t"
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-target.h b/firmware/target/arm/imx31/gigabeat-s/system-target.h
index 1f7a2475af..327d72b7bc 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/system-target.h
@@ -33,24 +33,28 @@ static inline void udelay(unsigned int usecs)
 #define __dbg_hw_info(...) 0
 #define __dbg_ports(...) 0
 
+void system_prepare_fw_start(void);
+void tick_stop(void);
+
 #define HAVE_INVALIDATE_ICACHE
 static inline void invalidate_icache(void)
 {
-    long rd = 0;
     asm volatile(
-        "mcr p15, 0, %0, c7, c10, 0 \n"
+        /* Clean and invalidate entire data cache */
+        "mcr p15, 0, %0, c7, c14, 0 \n"
+        /* Invalidate entire instruction cache */
         "mcr p15, 0, %0, c7, c5, 0  \n"
-        : : "r"(rd)
+        : : "r"(0)
     );
 }
 
 #define HAVE_FLUSH_ICACHE
 static inline void flush_icache(void)
 {
-    long rd = 0;
     asm volatile (
+        /* Clean entire data cache */
         "mcr p15, 0, %0, c7, c10, 0 \n"
-        : : "r"(rd)
+        : : "r"(0)
     );
 }