Gigabeat S: Interrupt enabled bootloader. Miscellaneous integration changes.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@16243 a1c6a512-1295-4272-9138-f99709370657

15 files changed, 283 insertions, 194 deletions

diff --git a/bootloader/gigabeat-s.c b/bootloader/gigabeat-s.c
index a4e6391c7e..f3e2917131 100644
--- a/bootloader/gigabeat-s.c
+++ b/bootloader/gigabeat-s.c
@@ -52,16 +52,27 @@ char buf[MAX_PATH];
 char basedir[] = "/Content/0b00/00/"; /* Where files sent via MTP are stored */
 char model[5];
 int (*kernel_entry)(void);
+extern void reference_system_c(void);
+
+/* Dummy stub that creates C references for C functions only used by
+   assembly - never called */
+void reference_files(void)
+{
+    reference_system_c();
+}
 
 void main(void)
 {
     lcd_clear_display();
     printf("Hello world!");
-    printf("Gigabeat S Rockbox Bootloader v.00000002");
+    printf("Gigabeat S Rockbox Bootloader v.00000003");
+    system_init();
     kernel_init();
     printf("kernel init done");
     int rc;
 
+    set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS);
+
     rc = ata_init();
     if(rc)
     {
@@ -109,11 +120,13 @@ void main(void)
     printf("Loading firmware");
 
     unsigned char *loadbuffer = (unsigned char *)0x0;
-    int buffer_size = 1024*1024;
+    int buffer_size = 31*1024*1024;
 
     rc = load_firmware(loadbuffer, buf, buffer_size);
     if(rc < 0)
-        error(buf, rc);
+        error((int)buf, rc);
+
+    system_prepare_fw_start();
 
     if (rc == EOK)
     {
diff --git a/firmware/app.lds b/firmware/app.lds
index 7fabf3563b..d2a9582a3d 100644
--- a/firmware/app.lds
+++ b/firmware/app.lds
@@ -41,7 +41,8 @@ INPUT(target/sh/crt0.o)
 #define DRAMSIZE (MEMORYSIZE * 0x100000) - 0x100 - PLUGINSIZE - STUBOFFSET - CODECSIZE - LCD_BUFFER_SIZE - TTB_SIZE
 #elif CONFIG_CPU==IMX31L
 #include "imx31l.h"
-#define DRAMSIZE (MEMORYSIZE * 0x100000) - PLUGINSIZE - STUBOFFSET - CODECSIZE - TTB_SIZE
+/* Subtract 1MB for the FRAME/TTB section */
+#define DRAMSIZE (MEMORYSIZE * 0x100000 - 0x100000) - PLUGINSIZE - STUBOFFSET - CODECSIZE
 #elif CONFIG_CPU==DM320
 #include "dm320.h"
 #define DRAMSIZE (MEMORYSIZE * 0x100000) - PLUGINSIZE - STUBOFFSET - CODECSIZE - LCD_BUFFER_SIZE - TTB_SIZE
diff --git a/firmware/boot.lds b/firmware/boot.lds
index c0f19bc430..fc876b1d20 100644
--- a/firmware/boot.lds
+++ b/firmware/boot.lds
@@ -80,7 +80,10 @@ INPUT(target/sh/crt0.o)
 #define FLASHORIG 0x001f0000
 #define FLASHSIZE 2M
 #elif CONFIG_CPU == IMX31L
-#define DRAMORIG 0x80000000
+#define DRAMORIG (0x02000000-0x00100000)
+#undef  DRAMSIZE
+#define DRAMSIZE (1 << 20) /* Limit 1 MB for bootloader */
+#define IRAM DRAM
 #define IRAMORIG 0x1FFFC000
 #define IRAMSIZE 16K
 #define FLASHORIG 0x0000000
@@ -115,7 +118,12 @@ MEMORY
 #endif
    IRAM : ORIGIN = IRAMORIG, LENGTH = IRAMSIZE
 }
-#elif !defined(CPU_PP) && (CONFIG_CPU!=S3C2440) && (CONFIG_CPU!=IMX31L)
+#elif CONFIG_CPU == IMX31L
+MEMORY
+{
+   DRAM : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
+}
+#elif !defined(CPU_PP) && (CONFIG_CPU!=S3C2440)
 MEMORY
 {
    DRAM : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
@@ -302,48 +310,66 @@ SECTIONS
 }
 #elif (CONFIG_CPU==IMX31L)
 {
-#if 0
+    . = DRAMORIG;
+
+    .text :
+    {
+        *(.init.text)
+        *(.text*)
+	*(.icode)
+        *(.glue_7)
+        *(.glue_7t)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    .rodata :
+    {
+        *(.rodata)  /* problems without this, dunno why */
+        *(.rodata*)
+        *(.rodata.str1.1)
+        *(.rodata.str1.4)
+        . = ALIGN(0x4);
+
+        /* Pseudo-allocate the copies of the data sections */
+        _datacopy = .;
+    } > DRAM
+
+    .data :
+    {
+        *(.irodata)
+        *(.idata)
+        *(.data*)
+        . = ALIGN(0x4);
+        _dataend = . ;
+    } > DRAM
+
+    .stack :
+    {
+        *(.stack)
+        _stackbegin = .;
+        stackbegin = .;
+        . += 0x2000;
+        _stackend = .;
+        stackend = .;
+    } > IRAM
+
+    .bss :
+    {
+         _edata = .;
+         *(.bss*);
+         *(.ibss);
+         *(COMMON)
+         _end = .;
+    } > DRAM
+
     .vectors 0x0 :
     {
         _vectorsstart = .;
         *(.vectors);
+        KEEP(*(.vectors));
         _vectorsend = .;
-    } AT> DRAM
-#endif
-    . = 0x82000000;
-    
-   .text :
-   {
-       *(.init.text)
-       *(.text*)
-   }
-
-   .data :
-   {
-       *(.icode)
-       *(.irodata)
-       *(.idata)
-       *(.data*)
-       _dataend = . ;
-   }
-
-   .stack :
-   {
-       *(.stack)
-       _stackbegin = .;
-       stackbegin = .;
-       . += 0x2000;
-       _stackend = .;
-       stackend = .;
-   }
-
-   .bss :
-   {
-        _edata = .;
-        *(.bss*);
-        *(.ibss);
-        _end = .;
-   }
+    } AT > DRAM
+    _vectorscopy = LOADADDR(.vectors);
 }
 #else
 {
diff --git a/firmware/export/imx31l.h b/firmware/export/imx31l.h
index 1712a5b8f2..b1f35e80f2 100755
--- a/firmware/export/imx31l.h
+++ b/firmware/export/imx31l.h
@@ -23,7 +23,8 @@
 #define REG16_PTR_T volatile unsigned short *
 #define REG32_PTR_T volatile unsigned long *
 
-#define TTB_BASE_ADDR           (0x80000000 + (64*1024*1024)-TTB_SIZE)
+/* Place in the section with the framebuffer */
+#define TTB_BASE_ADDR           (0x80100000 + 0x00100000 - TTB_SIZE)
 #define IRAM_BASE_ADDR          0x1fffc000
 #define L2CC_BASE_ADDR          0x30000000
  
@@ -275,6 +276,7 @@
 #define INTENABLEL             (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x14))
 #define INTTYPEH               (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x18))
 #define INTTYPEL               (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x1C))
+#define NIPRIORITY(n)          (((REG32_PTR_T)(AVIC_BASE_ADDR+0x20))[n])
 #define NIPRIORITY7            (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x20))
 #define NIPRIORITY6            (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x24))
 #define NIPRIORITY5            (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x28))
@@ -294,7 +296,7 @@
 #define FIPNDH                 (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x60))
 #define FIPNDL                 (*(REG32_PTR_T)(AVIC_BASE_ADDR+0x64))
 #define VECTOR_BASE_ADDR       (AVIC_BASE_ADDR+0x100)
-#define VECTOR(n)              (*(REG32_PTR_T)(VECTOR_BASE_ADDR+((n)*4)))
+#define VECTOR(n)              (((REG32_PTR_T)VECTOR_BASE_ADDR)[n])
 
 /* The vectors go all the way up to 63. 4 bytes for each */
 #define INTCNTL_ABFLAG         (1 << 25)
diff --git a/firmware/export/kernel.h b/firmware/export/kernel.h
index be5041f6c2..70a2f98d59 100644
--- a/firmware/export/kernel.h
+++ b/firmware/export/kernel.h
@@ -157,8 +157,6 @@ struct event
 /* We don't enable interrupts in the iPod bootloader, so we need to fake
    the current_tick variable */
 #define current_tick (signed)(USEC_TIMER/10000)
-#elif (CONFIG_CPU == IMX31L) && defined(BOOTLOADER)
-#define current_tick (signed)((0xFFFFFFFF - EPITCNT1)/10000)
 #else
 extern volatile long current_tick;
 #endif
diff --git a/firmware/kernel.c b/firmware/kernel.c
index 8eba5651ad..835181f1ae 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -24,9 +24,6 @@
 #include "cpu.h"
 #include "system.h"
 #include "panic.h"
-#if CONFIG_CPU == IMX31L
-#include "avic-imx31.h"
-#endif
 
 /* Make this nonzero to enable more elaborate checks on objects */
 #ifdef DEBUG
@@ -42,7 +39,7 @@
 #define KERNEL_ASSERT(exp, msg...) ({})
 #endif
 
-#if (!defined(CPU_PP) && (CONFIG_CPU != IMX31L)) || !defined(BOOTLOADER) 
+#if !defined(CPU_PP) || !defined(BOOTLOADER) 
 volatile long current_tick NOCACHEDATA_ATTR = 0;
 #endif
 
@@ -107,7 +104,7 @@ void sleep(int ticks)
 
 void yield(void)
 {
-#if ((CONFIG_CPU == S3C2440 || defined(ELIO_TPJ1022) || CONFIG_CPU == IMX31L) && defined(BOOTLOADER))
+#if ((CONFIG_CPU == S3C2440 || defined(ELIO_TPJ1022)) && defined(BOOTLOADER))
     /* Some targets don't like yielding in the bootloader */
 #else
     switch_thread(NULL);
diff --git a/firmware/system.c b/firmware/system.c
index 2ba31f254f..65478e724b 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -273,5 +273,11 @@ void __div0(void)
 }
 #endif
 
+#ifdef BOOTLOADER
+void reference_system_c(void)
+{
+}
+#endif
+
 #endif /* CPU_ARM */
 
diff --git a/firmware/target/arm/imx31/crt0.S b/firmware/target/arm/imx31/crt0.S
index 9d50888536..4e79cd1352 100644
--- a/firmware/target/arm/imx31/crt0.S
+++ b/firmware/target/arm/imx31/crt0.S
@@ -23,9 +23,16 @@
 
     .global    start
 start:
-    b          newstart
-    .space 4*12 /* Space for low vectors */
-
+    /* Exception vectors */
+    b       newstart
+    b       undef_instr_handler
+    b       software_int_handler
+    b       prefetch_abort_handler
+    b       data_abort_handler
+    b       reserved_handler
+    b       irq_handler
+    b       fiq_handler
+    .balign 0x40, 0x6B
 
 /* Arm bootloader and startup code based on startup.s from the iPodLinux loader
  *
@@ -34,28 +41,41 @@ start:
  *
  */
 
+/* Initially this code is running at VA 0x8a000000 (PA 0x82000000).
+ * The mapping stub is copied to IRAM (0x1fffc000) and jumps to the final
+ * VA remapping starting at 0x01f00000 because the 1MB section containing
+ * the framebuffer at PA 0x81000000 is skipped in the remapping giving 63MB
+ * of contiguous RAM for the firmware. The TTB is placed at the end of said
+ * section.
+ *
+ * For now this will be done in bootloader, especially if usb will be needed
+ * within the bootloader to load the main firmware file. Interrupts will be
+ * needed for this (whether they be swi or irq).
+ */
 newstart:
     msr     cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */
 
 #ifdef BOOTLOADER
-    ldr     r2, =remap_start
-    ldr     r3, =remap_end
+    adr     r2, remap_start    /* Load PC-relative labels */
+    adr     r3, remap_end
     ldr     r5, =TTB_BASE_ADDR /* TTB pointer */
     ldr     r6, =IRAM_BASE_ADDR
     mov     r1, r6
 
-1:    
+1:  /* Copy remapping stub to IRAM */
     cmp     r3, r2
     ldrhi   r4, [r2], #4
     strhi   r4, [r1], #4
     bhi     1b
 
     mov     pc, r6
-    
+
+    /* Remapping stub. No absolute addresses may be used until after the
+     * remapping is complete. */
 remap_start:
     mrc     p15, 0, r3, c1, c0, 0  /* perform writeback if D cache is enabled */
-    tst     r3, #(1 <<  2)
-    tsteq   r3, #(1 << 12)
+    tst     r3, #(1 <<  2)         /* dcache? */
+    tsteq   r3, #(1 << 12)         /* or icache? */
     mcrne   p15, 0, r0, c7, c10, 0 /* clean dcache */
     mov     r0, #0
     mcrne   p15, 0, r0, c7, c7, 0  /* invalidate I cache and D cache */
@@ -77,9 +97,9 @@ remap_start:
     str     r1, [r0, #L2_CACHE_CTL_REG]
 
     /* Disble L1 caches and memory manager */
-    bic     r3, r3, #(1 <<  1)
-    bic     r3, r3, #(1 <<  2)
-    bic     r3, r3, #(1 << 12)
+    bic     r3, r3, #(1 << 12)    /* L1 I-cache disabled */
+    bic     r3, r3, #((1 << 2) |  /* L1 D-cache disabled */ \
+                      (1 << 0))   /* MMU disabled */
     mcr     p15, 0, r3, c1, c0, 0
     
     /*
@@ -96,92 +116,97 @@ remap_start:
     /* Invalidate L2 */
     mov     r1, #0x000000FF
     str     r1, [r0, #L2_CACHE_INV_WAY_REG]
-L2_loop:
+1:
     /* Poll Invalidate By Way register */
-    ldr     r2, [r0, #L2_CACHE_INV_WAY_REG]
-    cmp     r2, #0
-    bne     L2_loop
+    ldr     r1, [r0, #L2_CACHE_INV_WAY_REG]
+    cmp     r1, #0
+    bne     1b
 
     /*** End of L2 operations ***/
 
-    /*remap memory as well as exception vectors*/
-    /*for now this will be done in bootloader, especially
-      if usb will be needed within the bootloader to load the 
-      main firmware file. Interrupts will be needed for this
-      (whether they be swi or irq)*/
-
     /* TTB Initialisation */
-    mov     r3, r5
-    add     r2, r3, #TTB_SIZE
+    mov     r2, r5
+    add     r3, r5, #TTB_SIZE
     mov     r1, #0
-ttbloop:
-    str     r1, [r3], #4
-    cmp     r3, r2
-    bne     ttbloop
+1:
+    str     r1, [r2], #4
+    cmp     r2, r3
+    blo     1b
 
     /* Set TTB base address */
-    mov     r3, r5
-    mcr     p15, 0, r3, c2, c0, 0
+    mcr     p15, 0, r5, c2, c0, 0
+
     /* Set all domains to manager status */
-    mvn     r3, #0
-    mcr     p15, 0, r3, c3, c0, 0
+    mvn     r0, #0
+    mcr     p15, 0, r0, c3, c0, 0
 
     /* Set page tables */
 
     /* Map each memory loc to itself, no cache */
-    mov     r1, #0 /* Physical address */
-    mov     r3, r5
-    add     r4, r3, #TTB_SIZE                       /* End position */
-maploop1:
-    mov     r2, r1
-    orr     r2, r2, #(1<<10)    /* superuser - r/w, user - no access */
-    //orr   r2, r2, #(0<<5)     /* domain 0th */
-    orr     r2, r2, #(1<<4)     /* should be "1" */
-    orr     r2, r2, #(1<<1)     /* Section signature */
-    str     r2, [r3], #4
-    add     r1, r1, #(1<<20)
-    cmp     r3, r4
-    bne     maploop1
+                                   /* Physical address = 0x0 */
+    mov     r1,      #(1 << 10)    /* superuser - r/w, user - no access */
+    orr     r1, r1, #((0 <<  5)  | /* domain 0th */ \
+                      (1 <<  4)  | /* should be "1" */ \
+                      (1 <<  1))   /* Section signature */
+    mov     r2, r5
+    add     r3, r5, #TTB_SIZE   /* End position */
+1:
+    str     r1, [r2], #4
+    add     r1, r1, #(1 << 20)  /* Next MB */
+    cmp     r2, r3
+    blo     1b
+    sub     r1, r1, #TTB_SIZE/4*(1 << 20) /* Back up */
 
     /* Map 0x80000000 -> 0x0, cached */
-    mov     r1, #0x80000000 /* Physical address */
-    mov     r3, r5          /* TTB pointer */
-    add     r4, r3, #64*4   /* End position */
-maploop2:
-    mov     r2, r1
-    orr     r2, r2, #(1<<10)    /* superuser - r/w, user - no access */
-    //orr   r2, r2, #(0<<5)     /* domain 0th */
-    orr     r2, r2, #(1<<4)     /* should be "1" */
-    orr     r2, r2, #(1<<3)     /* cache flags */
-    orr     r2, r2, #(1<<2)     /* more cache stuff */
-    orr     r2, r2, #(1<<1)     /* Section signature */
-    str     r2, [r3], #4
-    add     r1, r1, #(1<<20)
-    bic     r6, r1, #0xf0000000
-    cmp     r6, #0x00100000     /* Skip framebuffer */
-    addeq   r1, r1, #(1<<20)
-    cmp     r3, r4
-    bne     maploop2
+    mov     r2, r5               /* TTB pointer */
+    add     r3, r5, #63*4        /* End position */
+    orr     r1, r1, #0x80000000  /* Physical address */
+    orr     r1, r1, #((1 << 3) | /* cache flag */ \
+                      (1 << 2))  /* buffer flag */
+1:
+    str     r1, [r2], #4
+    add     r1, r1, #(1 << 20)
+    and     r4, r1, #0x0ff00000
+    cmp     r4, #0x00100000      /* Skip framebuffer */
+    addeq   r1, r1, #(1 << 20)
+    cmp     r2, r3
+    blo     1b
 
     /* Enable MMU */
 
     mov     r0, #0
     mcr     p15, 0, r0, c8, c7, 0 /* Invalidate TLB */
     mcr     p15, 0, r0, c7, c7, 0 /* Invalidate icache and dcache */
-#if 1
+    
+    /* Auxilliary control register */
     mrc     p15, 0, r0, c1, c0, 1
-    bic     r0, r0, #0x70
-    bic     r0, r0, #0x07    
+    bic     r0, r0, #((1 << 6) | /* Restrict cache size OFF */ \
+                      (1 << 5) | /* Enable block tranfer cache operations */ \
+                      (1 << 4) | /* Clean+Invalidate cache operation ON */ \
+                      (1 << 3))  /* Round-robin micro TLB replacement */
+    orr     r0, r0, #((1 << 2) | /* Static branch prediction ON */ \
+                      (1 << 1) | /* Dynamic branch prediction ON */ \
+                      (1 << 0))  /* Return stack enabled */
     mcr     p15, 0, r0, c1, c0, 1
-#endif
+
+    /* Control register */
     mrc     p15, 0, r0, c1, c0, 0
-    orr     r0, r0, #(1 <<  0)    /* enable mmu bit */
-    orr     r0, r0, #(1 <<  2)    /* enable dcache */
-    bic     r0, r0, #(1 << 11)    /* no program flow prediction */
-    orr     r0, r0, #(1 << 12)    /* enable icache */
-    bic     r0, r0, #(1 << 13)    /* low vectors */
-    orr     r0, r0, #(1 << 14)    /* Round-robin */
-    bic     r0, r0, #(1 << 21)    /* No low latency interrupt */
+    bic     r0, r0, #((1 << 29) | /* AF by AP disabled */ \
+                      (1 << 28) | /* TEX remap disabled */ \
+                      (1 << 24) | /* Vectored interrupt OFF */ \
+                      (1 << 23) | /* Sub AP bits enabled (compatible) */ \
+                      (1 << 22))  /* Unaligned access support disabled */
+    bic     r0, r0, #((1 << 21) | /* All performance features enabled */ \
+                      (1 << 15))  /* Loads to PC set T bit */
+    bic     r0, r0, #((1 << 13) | /* Low vectors */ \
+                      (1 << 11))  /* Program flow prediction disabled (for now) */
+    orr     r0, r0, #((1 << 14) | /* Round-robin replacement for I/D caches */ \
+                      (1 << 12) | /* L1 I-cache enabled */ \
+                      (1 <<  9) | /* ROM protection enabled */ \
+                      (1 <<  8))  /* MMU protection enabled */
+    orr     r0, r0, #((1 <<  2) | /* L1 D-cache enabled */ \
+                      (1 <<  1) | /* Strict alignment enabled */ \
+                      (1 <<  0))  /* MMU enabled */
     mcr     p15, 0, r0, c1, c0, 0
     nop
     nop
@@ -194,8 +219,8 @@ remap_end:
 
 #endif /* BOOTLOADER */
 
-#ifndef BOOTLOADER
-    /* Copy exception handler code to address 0 */
+#ifdef BOOTLOADER
+    /* Copy bootloader exception handler code to address 0 */
     ldr     r2, =_vectorsstart
     ldr     r3, =_vectorsend
     ldr     r4, =_vectorscopy
@@ -204,7 +229,7 @@ remap_end:
     ldrhi   r5, [r4], #4
     strhi   r5, [r2], #4
     bhi     1b
-
+#else
     /* Zero out IBSS */
     ldr     r2, =_iedata
     ldr     r3, =_iend
@@ -261,47 +286,41 @@ remap_end:
     msr     cpsr_c, #0xd3
     bl      main
 
-/* Exception handlers. Will be copied to address 0 after memory remapping */
-_vectorstart:
+#ifdef BOOTLOADER
+    /* Exception vectors with absolute jumps for bootloader */
     .section .vectors,"aw"
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-    ldr    pc, [pc, #24]
-
-    /* Exception vectors */
-    .global vectors
-vectors:
-    .word  start 
-    .word  undef_instr_handler
-    .word  software_int_handler
-    .word  prefetch_abort_handler
-    .word  data_abort_handler
-    .word  reserved_handler
-    .word  irq_handler
-    .word  fiq_handler
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    ldr     pc, [pc, #24]
+    .word   newstart
+    .word   undef_instr_handler
+    .word   software_int_handler
+    .word   prefetch_abort_handler
+    .word   data_abort_handler
+    .word   reserved_handler
+    .word   irq_handler
+    .word   fiq_handler
+#endif /* BOOTLOADER */
 
     .text
-
     .global UIE
 
 /* All illegal exceptions call into UIE with exception address as first
-   parameter. This is calculated differently depending on which exception
-   we're in. Second parameter is exception number, used for a string lookup
-   in UIE.
- */
+ * parameter. This is calculated differently depending on which exception
+ * we're in. Second parameter is exception number, used for a string lookup
+ * in UIE. */
 undef_instr_handler:
     mov    r0, lr
     mov    r1, #0
     b      UIE
 
 /* We run supervisor mode most of the time, and should never see a software
-   exception being thrown. Perhaps make it illegal and call UIE?
- */
+ * exception being thrown. Perhaps make it illegal and call UIE? */
 software_int_handler:
 reserved_handler:
     movs   pc, lr
@@ -316,11 +335,6 @@ data_abort_handler:
     mov    r1, #2
     b      UIE
 
-#ifdef BOOTLOADER
-UIE:
-    b      UIE
-#endif    
-
 /* 256 words of IRQ stack */
     .space 256*4
 irq_stack:
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)
     );
 }
 
diff --git a/tools/configure b/tools/configure
index f0004fd2ba..52d48e2c79 100755
--- a/tools/configure
+++ b/tools/configure
@@ -1213,7 +1213,7 @@ EOF
     target_id=26
     modelname="gigabeats"
     target="-DGIGABEAT_S"
-    memory=32 # always
+    memory=64
     arm9tdmicc
     tool="$rootdir/tools/scramble -add=gigs"
     bmp2rb_mono="$rootdir/tools/bmp2rb -f 0"
diff --git a/tools/scramble.c b/tools/scramble.c
index b5aba3fbaa..3e5a0771bc 100644
--- a/tools/scramble.c
+++ b/tools/scramble.c
@@ -107,7 +107,7 @@ void usage(void)
            "\t-add=X  Rockbox generic \"add-up\" checksum format\n"
            "\t        (X values: h100, h120, h140, h300, ipco, nano, ipvd, mn2g\n"
            "\t                   ip3g, ip4g, mini, iax5, h10, h10_5gb, tpj2,\n"
-           "\t                   c200, e200, giga, m100, m500)\n"
+           "\t                   c200, e200, giga, gigs, m100, m500)\n"
            "\nNo option results in Archos standard player/recorder format.\n");
 
     exit(1);