1 files changed, 165 insertions, 4 deletions

diff --git a/firmware/target/arm/crt0.S b/firmware/target/arm/crt0.S
index 56876ca9b1..d734f82df8 100644
--- a/firmware/target/arm/crt0.S
+++ b/firmware/target/arm/crt0.S
@@ -23,6 +23,9 @@
 
     .global    start
 start:
+    b      newstart     
+    .space 4*16
+
 
 /* Arm bootloader and startup code based on startup.s from the iPodLinux loader
  *
@@ -31,7 +34,13 @@ start:
  *
  */
 
-    msr    cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ */
+newstart:
+#if CONFIG_CPU == IMX31L
+    mov r0,#0xD3
+    msr cpsr, r0
+#else
+    msr    cpsr, #0xd3 /* enter supervisor mode, disable IRQ */
+#endif  
 
 #if !defined(BOOTLOADER) || (CONFIG_CPU == DM320)
 #if !defined(DEBUG)
@@ -95,7 +104,7 @@ start:
     cmp    r3, r2
     strhi  r4, [r2], #4
     bhi    1b
-
+    
 #ifdef BOOTLOADER
 /* Code for ARM bootloader targets other than iPod go here */
 
@@ -274,6 +283,151 @@ start:
 start_loc:
      bl main
      
+#elif CONFIG_CPU == IMX31L
+          
+    mov r0, #0
+    mcr 15, 0, r0, c7, c7, 0        /* invalidate I cache and D cache */
+    mcr 15, 0, r0, c8, c7, 0        /* invalidate TLBs */
+    mcr 15, 0, r0, c7, c10, 4        /* Drain the write buffer */
+
+    /* Also setup the Peripheral Port Remap register inside the core */
+    ldr r0, =0x40000015        /* start from AIPS 2GB region */
+    mcr p15, 0, r0, c15, c2, 4
+
+    /*** L2 Cache setup/invalidation/disable ***/
+    /* Disable L2 cache first */
+    ldr r0, =L2CC_BASE_ADDR
+    ldr r2, [r0, #L2_CACHE_CTL_REG]
+    bic r2, r2, #0x1
+    str r2, [r0, #L2_CACHE_CTL_REG]
+
+
+    /*
+     * Configure L2 Cache:
+     * - 128k size(16k way)
+     * - 8-way associativity
+     * - 0 ws TAG/VALID/DIRTY
+     * - 4 ws DATA R/W
+     */
+    ldr r1, [r0, #L2_CACHE_AUX_CTL_REG]
+    and r1, r1, #0xFE000000
+    ldr r2, =0x00030024
+    orr r1, r1, r2
+    str r1, [r0, #L2_CACHE_AUX_CTL_REG]
+
+    /* Invalidate L2 */
+    ldr r1, =0x000000FF
+    str r1, [r0, #L2_CACHE_INV_WAY_REG]
+L2_loop:
+    /* Poll Invalidate By Way register */
+    ldr r2, [r0, #L2_CACHE_INV_WAY_REG]
+    cmp r2, #0
+    bne L2_loop
+    /*** End of L2 operations ***/
+                 /* Set up stack for IRQ mode */ 
+        mov    r0,#0xd2
+    msr    cpsr, r0
+    ldr    sp, =irq_stack
+    /* Set up stack for FIQ mode */ 
+    mov    r0,#0xd1
+    msr    cpsr, r0
+    ldr    sp, =fiq_stack
+
+    /* Let abort and undefined modes use IRQ stack */
+    mov    r0,#0xd7
+    msr    cpsr, r0
+    ldr    sp, =irq_stack
+    mov    r0,#0xdb
+    msr    cpsr, r0
+    ldr    sp, =irq_stack
+    /* Switch to supervisor mode */
+    mov    r0,#0xd3
+    msr    cpsr, r0
+    ldr    sp, =stackend
+    
+    /*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)*/
+        bl memory_init
+        mov r0,#0
+        ldr r1,=_vectorstart
+        mov r2,#0
+        
+lp: ldr r3,[r1]
+        add r1,r1,#4
+        str r3,[r0]
+        add r0,r0,#4
+        add r2,r2,#1
+        cmp r2,#16
+        bne lp
+        bl main
+
+.section .vectors,"aw"
+_vectorstart:
+    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  irqz
+    .word  fiqz
+
+    .text
+    .global irq
+    .global fiq
+    .global UIE
+
+undef_instr_handler:
+    mov    r0, lr
+    mov    r1, #0
+    b      UIE
+
+software_int_handler:
+reserved_handler:
+        bl     irq_handler
+    movs   pc, lr
+
+prefetch_abort_handler:
+    sub    r0, lr, #4
+    mov    r1, #1
+    b      UIE
+
+data_abort_handler:
+    sub    r0, lr, #8 
+    mov    r1, #2
+    b      UIE
+
+/*not working....if we get here, let someone
+know....*/
+irqz: bl irq_handler
+fiqz: bl fiq_handler
+
+UIE:
+    b UIE
+
+/* 256 words of IRQ stack */
+    .space 256*4
+irq_stack:
+
+/* 256 words of FIQ stack */
+    .space 256*4
+fiq_stack:
+
 #else
     /* get the high part of our execute address */
     ldr    r2, =0xffffff00
@@ -295,11 +449,15 @@ start_loc:
     
 start_loc:
      bl main
+
 #endif
 
 #else /* BOOTLOADER */
     
-    /* 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 */ 
@@ -316,8 +474,11 @@ start_loc:
     ldr    sp, =stackend
     bl     main
     /* main() should never return */
-   
+
 /* Exception handlers. Will be copied to address 0 after memory remapping */
+#if CONFIG_CPU == IMX31L
+_vectorstart:
+#endif
     .section .vectors,"aw"
     ldr    pc, [pc, #24]
     ldr    pc, [pc, #24]