This is a big chunk of code necessary to prepare for Gigabeat F flash loading and and implementation of rolo. There should be no noticible changes for the user. A new bootloader is not needed.

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

6 files changed, 554 insertions, 338 deletions

diff --git a/firmware/export/s3c2440.h b/firmware/export/s3c2440.h
index 0e802b5c0e..fae4f02203 100644
--- a/firmware/export/s3c2440.h
+++ b/firmware/export/s3c2440.h
@@ -19,6 +19,12 @@
 #ifndef __S3C2440_H__
 #define __S3C2440_H__
 
+#define LCD_BUFFER_SIZE (320*240*2)
+#define TTB_SIZE (0x4000)
+/* must be 16Kb (0x4000) aligned */
+#define TTB_BASE   ((unsigned int *)(0x30000000 + (32*1024*1024) - TTB_SIZE)) /* End of memory */
+#define FRAME   ((short *) ((char *)TTB_BASE - LCD_BUFFER_SIZE))  /* Right before TTB */
+
 /* Memory Controllers */
 
 #define BWSCON (*(volatile int *)0x48000000) /* Bus width & wait status control */
@@ -225,12 +231,6 @@
 #define LCDINTMSK (*(volatile int *)0x4D00005C) /* LCD interrupt mask */
 #define TCONSEL (*(volatile int *)0x4D000060) /* TCON(LPC3600/LCC3600) control */
 
-#define LCD_BUFFER_SIZE ((320*240*2))
-#define TTB_SIZE (0x4000)
-/*#define FRAME   ( (short *) 0x31E00000  ) */ /* LCD Frame buffer - Firmware Address */
-/* must be 16Kb (0x4000) aligned */
-#define TTB_BASE   ((unsigned int *)(0x30000000 + (32*1024*1024) - TTB_SIZE)) /* End of memory */
-#define FRAME   ((short *) ((char *)TTB_BASE - LCD_BUFFER_SIZE))  /* Right before TTB */
 /* NAND Flash */
 
 #define NFCONF (*(volatile int *)0x4E000000) /* NAND flash configuration */
diff --git a/firmware/target/arm/mmu-arm.c b/firmware/target/arm/mmu-arm.c
index ffca7a43ee..c4b63cf805 100644
--- a/firmware/target/arm/mmu-arm.c
+++ b/firmware/target/arm/mmu-arm.c
@@ -20,70 +20,111 @@
 #include "mmu-arm.h"
 #include "panic.h"
 
-#define SECTION_ADDRESS_MASK (-1 << 20)
-#define MB (1 << 20)
+void __attribute__((naked)) ttb_init(void) {
+    asm volatile
+    (
+        "mcr p15, 0, %[ttbB], c2, c0, 0 \n" /* Set the TTB base address */
+        "mcr p15, 0, %[ffff], c3, c0, 0 \n" /* Set all domains to manager status */
+        "bx  lr                         \n"
+        : 
+        :   [ttbB] "r" (TTB_BASE), 
+            [ffff] "r" (0xFFFFFFFF)
+    );
+}
 
-void ttb_init(void) {
-    unsigned int* ttbPtr;
+void __attribute__((naked)) map_section(unsigned int pa, unsigned int va, int mb, int flags) {
+#if 0 /* This code needs to be fixed and the C needs to be replaced to ensure that stack is not used */
+    asm volatile
+    (
+        /* pa &= (-1 << 20);   // align to 1MB */
+        "mov  r0, r0, lsr #20   \n"
+        "mov  r0, r0, lsl #20   \n"
 
-    /* must be 16Kb (0x4000) aligned - clear out the TTB */
-    for (ttbPtr=TTB_BASE; ttbPtr<(TTB_SIZE+TTB_BASE); ttbPtr++)
-    {
-        *ttbPtr = 0;
-    }
+        /* pa |= (flags | 0x412);
+         * bit breakdown:
+         *  10:     superuser - r/w, user - no access
+         *  4:      should be "1"
+         *  3,2:    Cache flags (flags (r3))
+         *  1:      Section signature 
+         */
 
-    /* Set the TTB base address */
-    asm volatile("mcr p15, 0, %0, c2, c0, 0" : : "r" (TTB_BASE));
+        "orr  r0, r0, r3        \n"
+        "orr  r0, r0, #0x410    \n"
+        "orr  r0, r0, #0x2      \n"
+        :
+        :
+    );
 
-    /* Set all domains to manager status */
-    asm volatile("mcr p15, 0, %0, c3, c0, 0" : : "r" (0xFFFFFFFF));
-}
+    register int *ttb_base asm ("r3") = TTB_BASE;   /* force in r3 */
 
-void map_section(unsigned int pa, unsigned int va, int mb, int cache_flags) {
-    unsigned int* ttbPtr;
-    int i;
-    int section_no;
+    asm volatile
+    (
+        /* unsigned int* ttbPtr = TTB_BASE + (va >> 20);
+         * sections are 1MB size 
+         */
 
-    section_no = va >> 20; /* sections are 1Mb size */
-    ttbPtr = TTB_BASE + section_no;
-    pa &= SECTION_ADDRESS_MASK; /* align to 1Mb */
-    for(i=0; i<mb; i++, pa += MB) {
-        *(ttbPtr + i) =
-            pa |
-            1 << 10 |    /* superuser - r/w, user - no access */
-            0 << 5 |    /* domain 0th */
-            1 << 4 |    /* should be "1" */
-            cache_flags |
-            1 << 1;    /* Section signature */
-    }
-}
+        "mov  r1, r1, lsr #20           \n"
+        "add  r1, %[ttbB], r1, lsl #0x2 \n" 
 
-void enable_mmu(void) {
-    int regread;
+        /* Add MB to pa, flags are already present in pa, but addition 
+         *  should not effect them
+         *
+         * #define MB (1 << 20)
+         * for( ; mb>0; mb--, pa += MB)
+         * {
+         *     *(ttbPtr++) = pa;
+         * }
+         * #undef MB
+         */
 
-    asm volatile(
-        "MRC p15, 0, %r0, c1, c0, 0\n"  /* Read reg1, control register */
-    : /* outputs */
-        "=r"(regread)
-    : /* inputs */
-    : /* clobbers */
-        "r0"
+        "cmp  r2, #0                    \n"
+        "bxle lr                        \n"
+        "loop:                          \n"
+        "str  r0, [r1], #4              \n"
+        "add  r0, r0, #0x100000         \n"
+        "sub  r2, r2, #0x01             \n"
+        "bne  loop                      \n"
+        "bx   lr                        \n"
+        : 
+        :   [ttbB] "r" (ttb_base)   /* This /HAS/ to be in r3 */
     );
+    (void) pa;
+    (void) va;
+    (void) mb;
+    (void) flags;
+#else
+    pa &= (-1 << 20);
+    pa |= (flags | 0x412);
+    unsigned int* ttbPtr = TTB_BASE + (va >> 20);
 
-    if ( !(regread & 0x04) || !(regread & 0x00001000) ) /* Was the ICache or DCache Enabled? */
-        clean_dcache(); /* If so we need to clean the DCache before invalidating below */
-
-    asm volatile("mov r0, #0\n"
-        "mcr p15, 0, r0, c8, c7, 0\n" /* invalidate TLB */
-
-        "mcr p15, 0, r0, c7, c7,0\n" /* invalidate both icache and dcache */
+#define MB (1 << 20)
+    for( ; mb>0; mb--, pa += MB)
+    {
+        *(ttbPtr++) = pa;
+    }
+#undef MB
+#endif
+}
 
-        "mrc p15, 0, r0, c1, c0, 0\n"
-        "orr r0, r0, #1<<0\n" /* enable mmu bit, icache and dcache */
-        "orr r0, r0, #1<<2\n" /* enable dcache */
-        "orr r0, r0, #1<<12\n" /* enable icache */
-        "mcr p15, 0, r0, c1, c0, 0" : : : "r0");
-    asm volatile("nop \n nop \n nop \n nop");
+void __attribute__((naked)) enable_mmu(void) {
+    asm volatile(
+        "mov r0, #0                 \n"
+        "mcr p15, 0, r0, c8, c7, 0  \n" /* invalidate TLB */
+        "mcr p15, 0, r0, c7, c7,0   \n" /* invalidate both icache and dcache */
+        "mrc p15, 0, r0, c1, c0, 0  \n"
+        "orr r0, r0, #1             \n" /* enable mmu bit, icache and dcache */
+        "orr r0, r0, #1<<2          \n" /* enable dcache */
+        "orr r0, r0, #1<<12         \n" /* enable icache */
+        "mcr p15, 0, r0, c1, c0, 0  \n"
+        "nop                        \n" 
+        "nop                        \n"
+        "nop                        \n"
+        "nop                        \n"
+        "bx  lr                     \n"
+        : 
+        : 
+        : "r0"
+    );
 }
 
 #if CONFIG_CPU == IMX31L
@@ -105,35 +146,36 @@ void invalidate_dcache_range(const void *base, unsigned int size) {
     unsigned int addr = (((int) base) & ~31);    /* Align start to cache line*/
     unsigned int end = ((addr+size) & ~31)+64;  /* Align end to cache line, pad */
     asm volatile(
-"inv_start: \n"
-    "mcr p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "add %0, %0, #32 \n"
-    "cmp %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
-    "addne %0, %0, #32 \n"
-    "cmpne %0, %1 \n"
-    "bne inv_start \n"
-    "mov %0, #0\n"
-    "mcr p15,0,%0,c7,c10,4\n"    /* Drain write buffer */
-        : : "r" (addr), "r" (end));
+        "inv_start: \n"
+        "mcr p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "add %0, %0, #32 \n"
+        "cmp %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "mcrne p15, 0, %0, c7, c14, 1 \n" /* Clean and invalidate this line */
+        "addne %0, %0, #32 \n"
+        "cmpne %0, %1 \n"
+        "bne inv_start \n"
+        "mov %0, #0\n"
+        "mcr p15,0,%0,c7,c10,4\n"    /* Drain write buffer */
+        : : "r" (addr), "r" (end)
+    );
 }
 #endif
 
@@ -239,41 +281,20 @@ void __attribute__((naked)) clean_dcache(void)
 /* Cleans entire DCache */
 void clean_dcache(void)
 {
-    unsigned int index, addr;
+    unsigned int index, addr, low;
 
-    for(index = 0; index <= 63; index++) {
-        addr = (0 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (1 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (2 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (3 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (4 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (5 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (6 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
-        addr = (7 << 5) | (index << 26);
-        asm volatile(
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
+    for(index = 0; index <= 63; index++) 
+    {
+        for(low = 0;low <= 7; low++)
+        {
+            addr = (index << 26) | (low << 5);
+            asm volatile
+            (
+                "mcr p15, 0, %[addr], c7, c10, 2 \n" /* Clean this entry by index */
+                : 
+                : [addr] "r" (addr)
+            );
+        }
     }
 }
 #endif
diff --git a/firmware/target/arm/mmu-arm.h b/firmware/target/arm/mmu-arm.h
index b744305dbd..4b8f3448d4 100644
--- a/firmware/target/arm/mmu-arm.h
+++ b/firmware/target/arm/mmu-arm.h
@@ -17,13 +17,16 @@
  *
  ****************************************************************************/
 
-#define CACHE_ALL (1 << 3 | 1 << 2 )
-#define CACHE_NONE 0
-#define BUFFERED (1 << 2)
+#define CACHE_ALL   0x0C
+#define CACHE_NONE  0
+#define BUFFERED    0x04
 
 void ttb_init(void);
 void enable_mmu(void);
-void map_section(unsigned int pa, unsigned int va, int mb, int cache_flags);
+void map_section(unsigned int pa, unsigned int va, int mb, int flags);
+
+/* Cleans entire DCache */
+void clean_dcache(void);
 
 /* Invalidate DCache for this range  */
 /* Will do write back */
@@ -37,7 +40,4 @@ void clean_dcache_range(const void *base, unsigned int size);
 /* Will *NOT* do write back */
 void dump_dcache_range(const void *base, unsigned int size);
 
-/* Cleans entire DCache */
-void clean_dcache(void);
-
 void memory_init(void);
diff --git a/firmware/target/arm/s3c2440/app.lds b/firmware/target/arm/s3c2440/app.lds
index 86f37d5a29..e6efe62656 100644
--- a/firmware/target/arm/s3c2440/app.lds
+++ b/firmware/target/arm/s3c2440/app.lds
@@ -16,12 +16,9 @@ INPUT(target/arm/s3c2440/crt0.o)
 #endif
 
 #include "cpu.h"
-#define DRAMSIZE (MEMORYSIZE * 0x100000) - 0x100 - STUBOFFSET - PLUGINSIZE - CODECSIZE - LCD_BUFFER_SIZE - TTB_SIZE
+#define DRAMSIZE (MEMORYSIZE * 0x100000) - STUBOFFSET - PLUGINSIZE - CODECSIZE - LCD_BUFFER_SIZE - TTB_SIZE
 
-#define DRAMORIG 0x00000100 + STUBOFFSET
-#define IRAMORIG DRAMORIG
-#define IRAM DRAM
-#define IRAMSIZE 0x1000
+#define DRAMORIG 0x00000000 + STUBOFFSET
 
 /* End of the audio buffer, where the codec buffer starts */
 #define ENDAUDIOADDR  (DRAMORIG + DRAMSIZE)
@@ -36,18 +33,26 @@ MEMORY
 
 SECTIONS
 {
-    .text :
+    .vectors DRAMORIG :
     {
-        loadaddress = .;
-        _loadaddress = .;
-        . = ALIGN(0x200);
+        _vectorstart = .;
+        *(.vectors*);
         *(.init.text)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    .text :
+    {
+        _textstart = .;
+        *(.text)
         *(.text*)
         *(.glue_7)
         *(.glue_7t)
         . = ALIGN(0x4);
     } > DRAM
 
+    _textcopy = LOADADDR(.text);
+
     .rodata :
     {
         *(.rodata)  /* problems without this, dunno why */
@@ -55,35 +60,18 @@ SECTIONS
         *(.rodata.str1.1)
         *(.rodata.str1.4)
         . = ALIGN(0x4);
-
-        /* Pseudo-allocate the copies of the data sections */
-        _datacopy = .;
     } > DRAM
 
-    /* TRICK ALERT! For RAM execution, we put the .data section at the
-       same load address as the copy. Thus, we don't waste extra RAM
-       when we don't actually need the copy.  */
-    .data : AT ( _datacopy )
+    .data :
     {
-        _datastart = .;
         *(.data*)
         . = ALIGN(0x4);
-        _dataend  = .;
     } > DRAM
 
     /DISCARD/ :
     {
         *(.eh_frame)
-    }
-
-    .vectors 0x0 :
-    {
-        _vectorsstart = .;
-        *(.vectors);
-        _vectorsend = .;
-    } AT> DRAM
-
-    _vectorscopy = LOADADDR(.vectors);
+    } 
 
     .iram :
     {
@@ -97,7 +85,7 @@ SECTIONS
 
     _iramcopy = LOADADDR(.iram);
 
-    .ibss (NOLOAD) :
+    .ibss :
     {
         _iedata = .;
         *(.ibss)
diff --git a/firmware/target/arm/s3c2440/boot.lds b/firmware/target/arm/s3c2440/boot.lds
index cc20fbe86f..4516c7ddb0 100644
--- a/firmware/target/arm/s3c2440/boot.lds
+++ b/firmware/target/arm/s3c2440/boot.lds
@@ -7,44 +7,93 @@ INPUT(target/arm/s3c2440/crt0.o)
 
 #define DRAMSIZE (MEMORYSIZE * 0x100000)
 
-#define DRAMORIG 0x30000000
-#define IRAMORIG 0x40000000
-#define IRAMSIZE 4K
-#define FLASHORIG 0x0000000
+#define DRAMORIG  0x00000000
+#define IRAMORIG  0x40000000
+#define IRAMSIZE  4K
+#define FLASHORIG 0x00000000
 #define FLASHSIZE 1M
 
-SECTIONS
+MEMORY
 {
-    . = DRAMORIG + 0x1000000;
+    DRAM : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
+}
 
-    .text : {
+SECTIONS
+{
+    .vectors DRAMORIG :
+    {
+        _vectorstart = .;
+        *(.vectors*);
         *(.init.text)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    .text :
+    {
+        _textstart = .;
+        *(.text)
         *(.text*)
-    }
+        *(.glue_7)
+        *(.glue_7t)
+        . = ALIGN(0x4);
+    } > DRAM
 
-    .data : {
+    _textcopy = LOADADDR(.text);
+
+    .rodata :
+    {
+        *(.rodata)  /* problems without this, dunno why */
+        *(.rodata*)
+        *(.rodata.str1.1)
+        *(.rodata.str1.4)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    .data :
+    {
+        *(.data*)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    /DISCARD/ :
+    {
+        *(.eh_frame)
+    } 
+
+    .iram :
+    {
+        _iramstart = .;
         *(.icode)
         *(.irodata)
         *(.idata)
-        *(.data*)
+        . = ALIGN(0x4);        
+        _iramend = .;
+    } > DRAM 
+
+    _iramcopy = LOADADDR(.iram);
+
+    .ibss :
+    {
+        _iedata = .;
+        *(.ibss)
         . = ALIGN(0x4);
-        _dataend = . ;
-     }
+        _iend = .;
+    } > DRAM 
 
     .stack :
     {
-         *(.stack)
-         _stackbegin = .;
-         stackbegin = .;
-         . += 0x2000;
-         _stackend = .;
-         stackend = .;
-    }
-    .bss : {
-         _edata = .;
-         *(.bss*);
-         *(.ibss);
-         *(COMMON)
-         _end = .;
-     }
+       *(.stack)
+       stackbegin = .;
+       . += 0x2000;
+       stackend = .;
+    } > DRAM 
+
+    .bss :
+    {
+       _edata = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(0x4);
+       _end = .;
+    } > DRAM
 }
diff --git a/firmware/target/arm/s3c2440/crt0.S b/firmware/target/arm/s3c2440/crt0.S
index 4f220f6c7f..7f1ebf8dec 100644
--- a/firmware/target/arm/s3c2440/crt0.S
+++ b/firmware/target/arm/s3c2440/crt0.S
@@ -7,7 +7,11 @@
  *                     \/            \/     \/    \/            \/
  * $Id$
  *
- * Copyright (C) 2002 by Linus Nielsen Feltzing
+ * Copyright (C) 2008 by Karl Kurbjun
+ *
+ * Arm bootloader and startup code based on startup.s from the iPodLinux loader
+ *  Copyright (c) 2003, Daniel Palffy (dpalffy (at) rainstorm.org)
+ *  Copyright (c) 2005, Bernard Leach <leachbj@bouncycastle.org>
  *
  * 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.
@@ -19,73 +23,183 @@
 #include "config.h"
 #include "cpu.h"
 
-    .section .init.text,"ax",%progbits
+/* Exception Handlers */
+.section .vectors,"ax",%progbits
+.code 32
 
-    .global    start
-start:
+.global vectors
+vectors:
+    b   start
+    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
+
+/*
+ * Function: code_copy
+ *  Variables:
+ *      r0 = from
+ *      r1 = to
+ *      r2 = length
+ */
 
-/* Arm bootloader and startup code based on startup.s from the iPodLinux loader
- *
- * Copyright (c) 2003, Daniel Palffy (dpalffy (at) rainstorm.org)
- * Copyright (c) 2005, Bernard Leach <leachbj@bouncycastle.org>
- *
+.section    .init.text, "ax", %progbits
+.align      0x04
+.global     word_copy
+.type       word_copy, %function
+word_copy: 
+    sub     r2, r2, #0x04
+    cmp     r2, #0
+    ldrge   r3, [r0], #4
+    strge   r3, [r1], #4
+    bgt     word_copy
+    bx lr
+.ltorg  
+.size       word_copy, .-word_copy
+
+/*
+ * Entry: start
+ *  Variables:
+ *      none
  */
-    msr    cpsr, #0xd3 /* enter supervisor mode, disable IRQ */
-
-#if !defined(BOOTLOADER)
-    /* Copy exception handler code to address 0 */
-    ldr    r2, =_vectorsstart
-    ldr    r3, =_vectorsend
-    ldr    r4, =_vectorscopy
-1:
-    cmp    r3, r2
-    ldrhi  r5, [r4], #4
-    strhi  r5, [r2], #4
-    bhi    1b
-#endif
 
-#if !defined(BOOTLOADER) && !defined(STUB)
-    /* Zero out IBSS */
-    ldr    r2, =_iedata
-    ldr    r3, =_iend
-    mov    r4, #0
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
+.section    .init.text,"ax",%progbits
+.code       32
+.align      0x04 /* Align */
+.global     start
+start:
+    msr     cpsr, #0xd3 /* enter supervisor mode, disable IRQ */
 
-    /* Copy the IRAM */
-    ldr    r2, =_iramcopy
-    ldr    r3, =_iramstart
-    ldr    r4, =_iramend
-1:
-    cmp    r4, r3
-    ldrhi  r5, [r2], #4
-    strhi  r5, [r3], #4
-    bhi    1b
-#endif /* !BOOTLOADER, !STUB */
+    /* Disable the watchdog */
+    ldr     r2, =0x00000000
+    mov     r1, #0x53000000
+    str     r2, [r1]
 
-    /* Initialise bss section to zero */
-    ldr    r2, =_edata
-    ldr    r3, =_end
-    mov    r4, #0
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
-    
-    /* Set up some stack and munge it with 0xdeadbeef */
-    ldr    sp, =stackend
-    mov    r3, sp
-    ldr    r2, =stackbegin
-    ldr    r4, =0xdeadbeef
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
+    /* Mask all Interupts to be safe */
+    ldr     r2, =0xFFFFFFFF
+    mov     r1, #0x4A000000
+    str     r2, [r1]
+
+    /* Submask too */
+    ldr     r2, =0x00003FFF
+    str     r2, [r1, #0x1C]
+
+    /* Check if loaded by the old bootloader or by the OF
+     * Be careful with code size above this as well.
+     */
+
+    /* Get the execute address (cannot be past 0x100 for this to work */
+    ldr     r0, =0xffffff00
+    and     r0, pc, r0
+
+    /* Calculate the length of the code needed to run/copy */
+    ldr     r1, = _vectorstart
+    ldr     r2, = _iramend
+    sub     r2, r2, r1
+
+    add     r3, r2, #0x30000000
+
+    /* Is there enough space to copy without overwriting? */
+    cmp     r0, r3
+
+    /* There's enough space, skip copying */
+    bgt     skipreset
+
+    /* Is this code running from 0x0?  If so skip copy. */
+    cmplt   r0, #0
+    beq     skipreset
     
-#ifdef BOOTLOADER
-    /* Proper initialization pulled from 0x5070 */
+    /* There's not enough space to copy without overwriting, copy to safe spot
+     * and reset
+     */
+    mov     r1, #0x31000000     /* copy location */
+    bl      word_copy
+
+    mov     pc, #0x31000000
+
+    skipreset:  
+
+    /* Initial Clock Setup */
+    mov     r2, #0x7
+    mov     r1, #0x4C000000
+    str     r2, [r1, #0x14]
+
+    mov     r2, #0x0
+    str     r2, [r1, #0x18]
+
+    ldr     r2, =0xFFFFFFFF
+    str     r2, [r1]
+
+    ldr     r2, =0x0003C042
+    str     r2, [r1, #0x08]
+
+    nop
+    nop
+    nop
+    nop
+    nop
+    nop
+    nop
+    nop
+
+    ldr     r2, =0x000C9042
+    str     r2, [r1, #0x04]
+
+    nop
+    nop
+    nop
+    nop
+    nop
+    nop
+    nop
+    nop
+
+    /* If we want to disable extraneous clocks, uncomment, but it can 
+     *  freeze the device 
+     */
+#if 0
+    ldr     r2, =0x6030
+    mov     r1, #0x4C000000
+    str     r2, [r1, #0x0C]
+#endif
+
+    /* set Bus to Asynchronous mode (full speed) */    
+    mov     r0, #0
+    mrc     p15, 0, r0, c1, c0, 0
+    ldr     r1,  =0xC0000000
+    orr     r0, r0, r1 
+    mcr     p15, 0, r0, c1, c0, 0
+
+    /* Setup MISCCR */
+    ldr     r2, =0x00613020
+    mov     r1, #0x56000000
+    str     r2, [r1, #0x80]
+
+    /* Setup some unknown outputs in GPB and GPH */
+    ldr     r2, [r1, #0x10] 
+    mov     r3, #0x05
+    orr     r2, r3, r2
+    str     r2, [r1, #0x10]
+
+    ldr     r2, [r1, #0x14]
+    mov     r3, #0x03
+    orr     r2, r3, r2
+    str     r2, [r1, #0x14]
+
+    ldr     r2, [r1, #0x70] 
+    mov     r3, #0x05
+    orr     r2, r3, r2
+    str     r2, [r1, #0x70]
+
+    ldr     r2, [r1, #0x74]
+    mov     r3, #0x03
+    orr     r2, r3, r2
+    str     r2, [r1, #0x74]
+
+    /* Memory setup (taken from 0x5070) */
 
     /* BWSCON
      *      Reserved 0
@@ -120,7 +234,7 @@ start:
      *      Disable wait 0
      *      Not using UB/LB 0
      */
-    ldr     r2,=0x01055102
+    ldr     r2, =0x01055102
     mov     r1, #0x48000000
     str     r2, [r1]
 
@@ -133,8 +247,8 @@ start:
      *      Chip select setup time: 1 clock 01
      *      Address setup time: 0 clock 00
      */
-    ldr     r2,=0x00000D60
-    str     r2, [r1, #4]
+    ldr     r2, =0x00000D60
+    str     r2, [r1, #0x04]
 
 
     /* BANKCON1 
@@ -146,8 +260,8 @@ start:
      *      Chip select setup time: 0 clocks 00
      *      Address setup time: 0 clocks 00
      */
-    ldr     r2,=0x00000000
-    str     r2, [r1, #8]
+    ldr     r2, =0x00000000
+    str     r2, [r1, #0x08]
 
     /* BANKCON2 
      *      Pagemode: normal (1 data) 00
@@ -158,17 +272,17 @@ start:
      *      Chip select setup time: 4 clocks 11
      *      Address setup time: 0 clocks 00
      */
-    ldr     r2,=0x00001FA0
+    ldr     r2, =0x00001FA0
     str     r2, [r1, #0xC]
 
     /* BANKCON3 */
-    ldr     r2,=0x00001D80
+    ldr     r2, =0x00001D80
     str     r2, [r1, #0x10]
     /* BANKCON4 */
     str     r2, [r1, #0x14]
 
     /* BANKCON5 */
-    ldr     r2,=0x00000000
+    ldr     r2, =0x00000000
     str     r2, [r1, #0x18]
 
     /* BANKCON6/7
@@ -181,13 +295,13 @@ start:
      *      Tacs:   0 clock     00
      *      MT:     Sync DRAM   11
      */
-    ldr     r2,=0x00018005
+    ldr     r2, =0x00018005
     str     r2, [r1, #0x1C]
     /* BANKCON7 */
     str     r2, [r1, #0x20]
 
     /* REFRESH */
-    ldr     r2,=0x00980501
+    ldr     r2, =0x00980501
     str     r2, [r1, #0x24]
 
     /* BANKSIZE
@@ -198,117 +312,158 @@ start:
      *      Reserved: 0         0
      *      BURST_EN: enabled   1
      */
-    ldr     r2,=0x00000090
+    ldr     r2, =0x00000090
     str     r2, [r1, #0x28]
 
     /* MRSRB6 */
-    ldr     r2,=0x00000030
+    ldr     r2, =0x00000030
     str     r2, [r1, #0x2C]
     /* MRSRB7 */
     str     r2, [r1, #0x30]
     
 #if 0
-    /* This next part I am not sure of the purpose */
-    
     /* GPACON */
-    mov    r2,#0x01FFFCFF
-    str    r2,=0x56000000
+    mov     r1, #0x56000000
+    ldr     r2, =0x01FFFCFF /* 0x01FFFCFF */
+    str     r2, [r1]
     
     /* GPADAT */
-    mov    r2,#0x01FFFEFF
-    str    r2,=0x56000004
+    ldr     r2, =0x01FFFEFF
+    str     r2, [r1, #0x04]
 
     /* MRSRB6 */
-    mov    r2,#0x00000000
-    str    r2,=0x4800002C
+    mov     r1, #0x48000000
+    mov     r2, #0x00000000
+    str     r2, [r1, #0x2C]
 
     /* GPADAT */
-    ldr     r2,=0x01FFFFFF
-    mov     r1, #0x56000000 
-    str     r2, [r1, #4]
+    mov     r1, #0x56000000
+    ldr     r2, =0x01FFFFFF     
+    str     r2, [r1, #0x04]
 
     /* MRSRB6 */
-    mov    r2,#0x00000030
-    str    r2,=0x4800002C
+    mov     r1, #0x48000000
+    mov     r2, #0x00000030
+    str     r2, [r1, #0x2C]
 
     /* GPACON */
-    mov    r2,#0x01FFFFFF
-    str    r2,=0x56000000
+    mov     r1, #0x56000000 
+    mov     r2, #0x01FFFFFF
+    str     r2, [r1]
 
     /* End of the unknown */
 #endif
 
-    /* get the high part of our execute address */
-    ldr    r2, =0xffffff00
-    and    r4, pc, r2
-
-    /* Copy bootloader to safe area - 0x31000000 */
-    mov    r5, #0x30000000
-    add    r5, r5, #0x1000000
-    ldr    r6, = _dataend
-    sub    r0, r6, r5       /* length of loader */
-    add    r0, r4, r0     /* r0 points to start of loader */
-1:
-    cmp    r5, r6
-    ldrcc  r2, [r4], #4
-    strcc  r2, [r5], #4
-    bcc    1b
-
-    ldr    pc, =start_loc    /* jump to the relocated start_loc:  */
+    /* The builds have two potential load addresses, one being from flash, 
+     * and the other from some "unknown" location right now the assumption
+     * is that the code is not at 0x3000000.
+     */   
+    /* get the high part of our execute address (where am I) */
+    ldr     r0, =0xfffff000
+    and     r0, pc, r0
+
+    /* Copy code to 0x30000000 */
+    ldr     r2, = _vectorstart
+    ldr     r3, = _iramend
+
+    sub     r2, r3, r2           /* length of loader */
+    
+    ldr     r1, =0x30000000     /* copy location */   
     
-start_loc:
-     bl main
+    bl      word_copy
+
+    ldr     r1, =donecopy
+    ldr     r2, =0x30000000
+    add     r1, r1, r2
+    mov     pc, r1           /* The code is located where we want it-jump*/
+
+donecopy: 
+
+    /* Setup the MMU, start by disabling */
 
-#else /* BOOTLOADER */
+    mrc     p15, 0, r0, c1, c0, 0  
+    bic     r0, r0, #0x41      /* disable mmu and dcache */
+    bic     r0, r0, #0x1000   /* disable icache */
+    mcr     p15, 0, r0, c1, c0, 0
+
+    bl      ttb_init
+    
+    ldr     r0, =0x0
+    ldr     r1, =0x0
+    ldr     r2, =0x1000
+    mov     r3, #0
+    bl      map_section
+
+    ldr     r0, =0x30000000
+    ldr     r1, =0x0
+    mov     r2, #32
+    mov     r3, #12
+    bl      map_section
+
+    ldr     r0, =0x31FD6800     /* FRAME */
+    mov     r1, r0
+    mov     r2, #1
+    mov     r3, #4
+    bl      map_section        
+    
+    bl      enable_mmu   
+
+    /* Zero out IBSS */
+    ldr     r2, =_iedata
+    ldr     r3, =_iend
+    mov     r4, #0
+ibsszero:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     ibsszero
+
+    /* Copy the IRAM */
+    ldr     r0, =_iramcopy
+    ldr     r1, =_iramstart
+    ldr     r2, =_iramend
+    sub     r2, r2, r1
+    bl      word_copy
+
+    /* Initialise bss section to zero */
+    ldr     r2, =_edata
+    ldr     r3, =_end
+    mov     r4, #0
+bsszero:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     bsszero
+    
+    /* Set up some stack and munge it with 0xdeadbeef */
+    ldr     sp, =stackend
+    mov     r3, sp
+    ldr     r2, =stackbegin
+    ldr     r4, =0xdeadbeef
+stackmunge:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     stackmunge
 
      /* Set up stack for IRQ mode */
-    msr    cpsr_c, #0xd2
-    ldr    sp, =irq_stack
+    msr     cpsr_c, #0xd2
+    ldr     sp, =irq_stack
     /* Set up stack for FIQ mode */ 
-    msr    cpsr_c, #0xd1
-    ldr    sp, =fiq_stack
+    msr     cpsr_c, #0xd1
+    ldr     sp, =fiq_stack
 
     /* Let abort and undefined modes use IRQ stack */
-    msr    cpsr_c, #0xd7
-    ldr    sp, =irq_stack
-    msr    cpsr_c, #0xdb
-    ldr    sp, =irq_stack
+    msr     cpsr_c, #0xd7
+    ldr     sp, =irq_stack
+    msr     cpsr_c, #0xdb
+    ldr     sp, =irq_stack
     /* Switch to supervisor mode */
-    msr    cpsr_c, #0xd3
-    ldr    sp, =stackend
-    bl     main
-    /* main() should never return */
-
-/* Exception handlers. Will be copied to address 0 after memory remapping */
-    .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
-
-    .text
-
-#ifndef STUB
-    .global irq
-    .global fiq
-    .global UIE
-#endif
+    msr     cpsr_c, #0xd3
+    ldr     sp, =stackend
+
+    /* Start the main function */
+    ldr     pc, =main
+
+    /* Should never get here, but let's restart in case */
+//    b       vectors
 
 /* All illegal exceptions call into UIE with exception address as first
    parameter. This is calculated differently depending on which exception
@@ -337,11 +492,15 @@ data_abort_handler:
     mov    r1, #2
     b      UIE
 
-#ifdef STUB
-UIE:
+#if defined(BOOTLOADER)
+fiq_handler:
     b UIE
 #endif
 
+UIE:
+    b UIE
+
+.section .text
 /* 256 words of IRQ stack */
     .space 256*4
 irq_stack:
@@ -350,4 +509,3 @@ irq_stack:
     .space 256*4
 fiq_stack:
 
-#endif /* BOOTLOADER */