3 files changed, 77 insertions, 5 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 3fe773babd..ecd5e69c22 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -657,6 +657,7 @@ target/arm/s3c2440/gigabeat-fx/wmcodec-meg-fx.c
 target/arm/lcd-as-memframe.S
 target/arm/mmu-arm.c
 target/arm/imx31/debug-imx31.c
+target/arm/imx31/rolo_restart.S
 target/arm/imx31/gigabeat-s/adc-imx31.c
 target/arm/imx31/gigabeat-s/ata-imx31.c
 target/arm/imx31/gigabeat-s/avic-imx31.c
diff --git a/firmware/rolo.c b/firmware/rolo.c
index 5dbb6f9874..06ae9e1380 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -108,10 +108,11 @@ static void rolo_error(const char *text)
     lcd_stop_scroll();
 }
 
-#if CONFIG_CPU == SH7034
-/* these are in assembler file "descramble.S" */
+#if CONFIG_CPU == SH7034 || CONFIG_CPU == IMX31L
+/* these are in assembler file "descramble.S" for SH7034 */
 extern unsigned short descramble(const unsigned char* source,
                                  unsigned char* dest, int length);
+/* this is in firmware/target/arm/imx31/rolo_restart.S for IMX31 */
 extern void rolo_restart(const unsigned char* source, unsigned char* dest,
                          int length);
 #else
@@ -169,7 +170,7 @@ void rolo_restart(const unsigned char* source, unsigned char* dest,
         "mov   pc, r0            \n"
     );
 
-#elif defined(CPU_TCC780X) || (CONFIG_CPU==IMX31L) || (CONFIG_CPU == S3C2440)
+#elif defined(CPU_TCC780X) || (CONFIG_CPU == S3C2440)
     /* Flush and invalidate caches */
     invalidate_icache();
 
@@ -284,9 +285,13 @@ int rolo_load(const char* filename)
     adc_close();
 
 #ifdef CPU_ARM
-    disable_fiq();
-#endif
+    /* Should do these together since some ARM version should never have
+     * FIQ disabled and not IRQ (imx31 errata). */
+    disable_interrupt(IRQ_FIQ_STATUS);
+#else
+    /* Some targets have a higher disable level than HIGEST_IRQ_LEVEL */
     set_irq_level(DISABLE_INTERRUPTS);
+#endif
 
 #elif CONFIG_CPU == SH7034
     /* Read file length from header and compare to real file length */
diff --git a/firmware/target/arm/imx31/rolo_restart.S b/firmware/target/arm/imx31/rolo_restart.S
new file mode 100644
index 0000000000..39053de96f
--- /dev/null
+++ b/firmware/target/arm/imx31/rolo_restart.S
@@ -0,0 +1,66 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Michael Sevakis
+ *
+ * RoLo restart code for IMX31
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "config.h"
+#include "cpu.h"
+
+/****************************************************************************
+ * void rolo_restart(const unsigned char* source, unsigned char* dest,
+ *                   int length) __attribute__((noreturn));
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     rolo_restart
+rolo_restart:
+    adr     r4, restart_copy_start
+    adr     r5, restart_copy_end
+    ldr     r6, =IRAM_BASE_ADDR
+    mov     r7, r6
+
+    @ Copy stub to IRAM  
+1:
+    ldr     r8, [r4], #4
+    str     r8, [r7], #4
+    cmp     r5, r4
+    bhi     1b
+
+    @ Branch to stub
+    bx      r6
+
+restart_copy_start:
+    @ Trivial copy of firmware to final location
+    mov     r4, r1
+1:
+    subs    r2, r2, #1
+    ldrb    r7, [r0], #1
+    strb    r7, [r4], #1
+    bge     1b
+
+    @ Clean and invalidate all caches
+    mov     r0, #0
+    mcr     p15, 0, r0, c7, c14, 0
+    mcr     p15, 0, r0, c7, c5, 0
+    mcr     p15, 0, r0, c7, c10, 4
+    mcr     p15, 0, r0, c7, c5, 4
+    
+    @ Branch to destination (should be address 0x00000000)
+    bx      r1
+restart_copy_end:
+    .size   rolo_restart,.-rolo_restart