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

apps/plugins/plugin.lds

@@ -30,12 +30,9 @@ OUTPUT_FORMAT(elf32-sh)
 #define NOCACHE_BASE 0x00000000
 #endif
 
-#if CONFIG_CPU==S3C2440
+#if CONFIG_CPU==DM320 || CONFIG_CPU==S3C2440
-#include "s3c2440.h"
+#include "cpu.h"
-#define DRAMSIZE (MEMORYSIZE * 0x100000) - 0x100 - PLUGIN_BUFFER_SIZE - STUBOFFSET - CODEC_SIZE - LCD_BUFFER_SIZE - TTB_SIZE
+#define DRAMSIZE (MEMORYSIZE * 0x100000) - STUBOFFSET - PLUGIN_BUFFER_SIZE - CODEC_SIZE - LCD_BUFFER_SIZE - TTB_SIZE
-#elif CONFIG_CPU==DM320
-#include "dm320.h"
-#define DRAMSIZE (MEMORYSIZE * 0x100000) - PLUGIN_BUFFER_SIZE - STUBOFFSET - CODEC_SIZE - LCD_BUFFER_SIZE - TTB_SIZE
 #elif CONFIG_CPU==IMX31L
 #include "imx31l.h"
 /* Reserve 1mb for LCD buffer/TTB as in app.lds */
@@ -65,7 +62,7 @@ OUTPUT_FORMAT(elf32-sh)
 #define IRAMORIG 0x407000
 #define IRAMSIZE 0x9000
 #elif CONFIG_CPU == S3C2440
-#define DRAMORIG 0x100 + STUBOFFSET
+#define DRAMORIG 0x0 + STUBOFFSET
 #define IRAMORIG DRAMORIG
 #define IRAMSIZE 4K
 #define IRAM DRAM

bootloader/gigabeat.c

@@ -54,7 +54,6 @@ void main(void)
     int rc;
     int(*kernel_entry)(void);
 
-    memory_init();
     power_init();
     system_init();
     lcd_init();
@@ -98,6 +97,8 @@ void main(void)
     printf("Rockbox boot loader");
     printf("Version %s", version);
 
+    sleep(50); /* ATA seems to error without this pause */
+
     rc = ata_init();
     if(rc)
     {
@@ -115,8 +116,8 @@ void main(void)
 
     printf("Loading firmware");
 
-    loadbuffer = (unsigned char*) 0x100;
+    loadbuffer = (unsigned char*) 0x31000000;
-    buffer_size = (unsigned char*)0x400000 - loadbuffer;
+    buffer_size = (unsigned char*)0x31400000 - loadbuffer;
 
     rc = load_firmware(loadbuffer, BOOTFILE, buffer_size);
     if(rc < 0)

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 */

firmware/target/arm/mmu-arm.c

@@ -20,70 +20,111 @@
 #include "mmu-arm.h"
 #include "panic.h"
 
-#define SECTION_ADDRESS_MASK (-1 << 20)
+void __attribute__((naked)) ttb_init(void) {
-#define MB (1 << 20)
+    asm volatile
-
+    (
-void ttb_init(void) {
+        "mcr p15, 0, %[ttbB], c2, c0, 0 \n" /* Set the TTB base address */
-    unsigned int* ttbPtr;
+        "mcr p15, 0, %[ffff], c3, c0, 0 \n" /* Set all domains to manager status */
-
+        "bx  lr                         \n"
-    /* must be 16Kb (0x4000) aligned - clear out the TTB */
+        : 
-    for (ttbPtr=TTB_BASE; ttbPtr<(TTB_SIZE+TTB_BASE); ttbPtr++)
+        :   [ttbB] "r" (TTB_BASE), 
-    {
+            [ffff] "r" (0xFFFFFFFF)
-        *ttbPtr = 0;
+    );
 }
 
-    /* Set the TTB base address */
+void __attribute__((naked)) map_section(unsigned int pa, unsigned int va, int mb, int flags) {
-    asm volatile("mcr p15, 0, %0, c2, c0, 0" : : "r" (TTB_BASE));
+#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"
 
-    /* Set all domains to manager status */
+        /* pa |= (flags | 0x412);
-    asm volatile("mcr p15, 0, %0, c3, c0, 0" : : "r" (0xFFFFFFFF));
+         * bit breakdown:
-}
+         *  10:     superuser - r/w, user - no access
+         *  4:      should be "1"
+         *  3,2:    Cache flags (flags (r3))
+         *  1:      Section signature 
+         */
 
-void map_section(unsigned int pa, unsigned int va, int mb, int cache_flags) {
+        "orr  r0, r0, r3        \n"
-    unsigned int* ttbPtr;
+        "orr  r0, r0, #0x410    \n"
-    int i;
+        "orr  r0, r0, #0x2      \n"
-    int section_no;
+        :
-
+        :
-    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 */
-    }
-}
-
-void enable_mmu(void) {
-    int regread;
-
-    asm volatile(
-        "MRC p15, 0, %r0, c1, c0, 0\n"  /* Read reg1, control register */
-    : /* outputs */
-        "=r"(regread)
-    : /* inputs */
-    : /* clobbers */
-        "r0"
     );
 
-    if ( !(regread & 0x04) || !(regread & 0x00001000) ) /* Was the ICache or DCache Enabled? */
+    register int *ttb_base asm ("r3") = TTB_BASE;   /* force in r3 */
-        clean_dcache(); /* If so we need to clean the DCache before invalidating below */
 
-    asm volatile("mov r0, #0\n"
+    asm volatile
+    (
+        /* unsigned int* ttbPtr = TTB_BASE + (va >> 20);
+         * sections are 1MB size 
+         */
+
+        "mov  r1, r1, lsr #20           \n"
+        "add  r1, %[ttbB], r1, lsl #0x2 \n" 
+
+        /* 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
+         */
+
+        "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);
+
+#define MB (1 << 20)
+    for( ; mb>0; mb--, pa += MB)
+    {
+        *(ttbPtr++) = pa;
+    }
+#undef MB
+#endif
+}
+
+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<<0\n" /* enable mmu bit, icache and dcache */
+        "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" : : : "r0");
+        "mcr p15, 0, r0, c1, c0, 0  \n"
-    asm volatile("nop \n nop \n nop \n nop");
+        "nop                        \n" 
+        "nop                        \n"
+        "nop                        \n"
+        "nop                        \n"
+        "bx  lr                     \n"
+        : 
+        : 
+        : "r0"
+    );
 }
 
 #if CONFIG_CPU == IMX31L
@@ -133,7 +174,8 @@ void invalidate_dcache_range(const void *base, unsigned int size) {
         "bne inv_start \n"
         "mov %0, #0\n"
         "mcr p15,0,%0,c7,c10,4\n"    /* Drain write buffer */
-        : : "r" (addr), "r" (end));
+        : : "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++) {
+    for(index = 0; index <= 63; index++) 
-        addr = (0 << 5) | (index << 26);
+    {
-        asm volatile(
+        for(low = 0;low <= 7; low++)
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
+        {
-            : : "r" (addr));
+            addr = (index << 26) | (low << 5);
-        addr = (1 << 5) | (index << 26);
+            asm volatile
-        asm volatile(
+            (
-            "mcr p15, 0, %0, c7, c10, 2 \n" /* Clean this entry by index */
+                "mcr p15, 0, %[addr], c7, c10, 2 \n" /* Clean this entry by index */
-            : : "r" (addr));
+                : 
-        addr = (2 << 5) | (index << 26);
+                : [addr] "r" (addr)
-        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));
     }
 }
 #endif

firmware/target/arm/mmu-arm.h

@@ -17,13 +17,16 @@
  *
  ****************************************************************************/
 
-#define CACHE_ALL (1 << 3 | 1 << 2 )
+#define CACHE_ALL   0x0C
 #define CACHE_NONE  0
-#define BUFFERED (1 << 2)
+#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);

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 DRAMORIG 0x00000000 + STUBOFFSET
-#define IRAMORIG DRAMORIG
-#define IRAM DRAM
-#define IRAMSIZE 0x1000
 
 /* End of the audio buffer, where the codec buffer starts */
 #define ENDAUDIOADDR  (DRAMORIG + DRAMSIZE)
@@ -36,18 +33,26 @@ MEMORY
 
 SECTIONS
 {
+    .vectors DRAMORIG :
+    {
+        _vectorstart = .;
+        *(.vectors*);
+        *(.init.text)
+        . = ALIGN(0x4);
+    } > DRAM
+
     .text :
     {
-        loadaddress = .;
+        _textstart = .;
-        _loadaddress = .;
+        *(.text)
-        . = ALIGN(0x200);
-        *(.init.text)
         *(.text*)
         *(.glue_7)
         *(.glue_7t)
         . = ALIGN(0x4);
     } > DRAM
 
+    _textcopy = LOADADDR(.text);
+
     .rodata :
     {
         *(.rodata)  /* problems without this, dunno why */
@@ -55,20 +60,12 @@ 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
+    .data :
-       same load address as the copy. Thus, we don't waste extra RAM
-       when we don't actually need the copy.  */
-    .data : AT ( _datacopy )
     {
-        _datastart = .;
         *(.data*)
         . = ALIGN(0x4);
-        _dataend  = .;
     } > DRAM
 
     /DISCARD/ :
@@ -76,15 +73,6 @@ SECTIONS
         *(.eh_frame)
     } 
 
-    .vectors 0x0 :
-    {
-        _vectorsstart = .;
-        *(.vectors);
-        _vectorsend = .;
-    } AT> DRAM
-
-    _vectorscopy = LOADADDR(.vectors);
-
     .iram :
     {
         _iramstart = .;
@@ -97,7 +85,7 @@ SECTIONS
 
     _iramcopy = LOADADDR(.iram);
 
-    .ibss (NOLOAD) :
+    .ibss :
     {
         _iedata = .;
         *(.ibss)

firmware/target/arm/s3c2440/boot.lds

@@ -7,44 +7,93 @@ INPUT(target/arm/s3c2440/crt0.o)
 
 #define DRAMSIZE (MEMORYSIZE * 0x100000)
 
-#define DRAMORIG 0x30000000
+#define DRAMORIG  0x00000000
 #define IRAMORIG  0x40000000
 #define IRAMSIZE  4K
-#define FLASHORIG 0x0000000
+#define FLASHORIG 0x00000000
 #define FLASHSIZE 1M
 
+MEMORY
+{
+    DRAM : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
+}
+
 SECTIONS
 {
-    . = DRAMORIG + 0x1000000;
+    .vectors DRAMORIG :
-
+    {
-    .text : {
+        _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 */
+        *(.rodata*)
+        *(.rodata.str1.1)
+        *(.rodata.str1.4)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    .data :
+    {
+        *(.data*)
+        . = ALIGN(0x4);
+    } > DRAM
+
+    /DISCARD/ :
+    {
+        *(.eh_frame)
     } 
 
-    .data : {
+    .iram :
+    {
+        _iramstart = .;
         *(.icode)
         *(.irodata)
         *(.idata)
-        *(.data*)
         . = ALIGN(0x4);        
-        _dataend = . ;
+        _iramend = .;
-     }
+    } > DRAM 
+
+    _iramcopy = LOADADDR(.iram);
+
+    .ibss :
+    {
+        _iedata = .;
+        *(.ibss)
+        . = ALIGN(0x4);
+        _iend = .;
+    } > DRAM 
 
     .stack :
     {
        *(.stack)
-         _stackbegin = .;
        stackbegin = .;
        . += 0x2000;
-         _stackend = .;
        stackend = .;
-    }
+    } > DRAM 
-    .bss : {
+
+    .bss :
+    {
        _edata = .;
-         *(.bss*);
+        *(.bss*)
-         *(.ibss);
         *(COMMON)
+        . = ALIGN(0x4);
        _end = .;
-     }
+    } > DRAM
 }

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 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
+ */
+
+.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
+ */
+
+.section    .init.text,"ax",%progbits
+.code       32
+.align      0x04 /* Align */
 .global     start
 start:
-
-/* 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>
- *
- */
     msr     cpsr, #0xd3 /* enter supervisor mode, disable IRQ */
 
-#if !defined(BOOTLOADER)
+    /* Disable the watchdog */
-    /* Copy exception handler code to address 0 */
+    ldr     r2, =0x00000000
-    ldr    r2, =_vectorsstart
+    mov     r1, #0x53000000
-    ldr    r3, =_vectorsend
+    str     r2, [r1]
-    ldr    r4, =_vectorscopy
+
-1:
+    /* Mask all Interupts to be safe */
-    cmp    r3, r2
+    ldr     r2, =0xFFFFFFFF
-    ldrhi  r5, [r4], #4
+    mov     r1, #0x4A000000
-    strhi  r5, [r2], #4
+    str     r2, [r1]
-    bhi    1b
+
+    /* 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
+    
+    /* 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
 
-#if !defined(BOOTLOADER) && !defined(STUB)
+    /* set Bus to Asynchronous mode (full speed) */    
-    /* Zero out IBSS */
+    mov     r0, #0
-    ldr    r2, =_iedata
+    mrc     p15, 0, r0, c1, c0, 0
-    ldr    r3, =_iend
+    ldr     r1,  =0xC0000000
-    mov    r4, #0
+    orr     r0, r0, r1 
-1:
+    mcr     p15, 0, r0, c1, c0, 0
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
 
-    /* Copy the IRAM */
+    /* Setup MISCCR */
-    ldr    r2, =_iramcopy
+    ldr     r2, =0x00613020
-    ldr    r3, =_iramstart
+    mov     r1, #0x56000000
-    ldr    r4, =_iramend
+    str     r2, [r1, #0x80]
-1:
-    cmp    r4, r3
-    ldrhi  r5, [r2], #4
-    strhi  r5, [r3], #4
-    bhi    1b
-#endif /* !BOOTLOADER, !STUB */
 
-    /* Initialise bss section to zero */
+    /* Setup some unknown outputs in GPB and GPH */
-    ldr    r2, =_edata
+    ldr     r2, [r1, #0x10] 
-    ldr    r3, =_end
+    mov     r3, #0x05
-    mov    r4, #0
+    orr     r2, r3, r2
-1:
+    str     r2, [r1, #0x10]
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
 
-    /* Set up some stack and munge it with 0xdeadbeef */
+    ldr     r2, [r1, #0x14]
-    ldr    sp, =stackend
+    mov     r3, #0x03
-    mov    r3, sp
+    orr     r2, r3, r2
-    ldr    r2, =stackbegin
+    str     r2, [r1, #0x14]
-    ldr    r4, =0xdeadbeef
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
 
-#ifdef BOOTLOADER
+    ldr     r2, [r1, #0x70] 
-    /* Proper initialization pulled from 0x5070 */
+    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
@@ -134,7 +248,7 @@ start:
      *      Address setup time: 0 clock 00
      */
     ldr     r2, =0x00000D60
-    str     r2, [r1, #4]
+    str     r2, [r1, #0x04]
 
 
     /* BANKCON1 
@@ -147,7 +261,7 @@ start:
      *      Address setup time: 0 clocks 00
      */
     ldr     r2, =0x00000000
-    str     r2, [r1, #8]
+    str     r2, [r1, #0x08]
 
     /* BANKCON2 
      *      Pagemode: normal (1 data) 00
@@ -208,58 +322,126 @@ start:
     str     r2, [r1, #0x30]
     
 #if 0
-    /* This next part I am not sure of the purpose */
-    
     /* GPACON */
-    mov    r2,#0x01FFFCFF
-    str    r2,=0x56000000
-    
-    /* GPADAT */
-    mov    r2,#0x01FFFEFF
-    str    r2,=0x56000004
-
-    /* MRSRB6 */
-    mov    r2,#0x00000000
-    str    r2,=0x4800002C
-
-    /* GPADAT */
-    ldr     r2,=0x01FFFFFF
     mov     r1, #0x56000000
-    str     r2, [r1, #4]
+    ldr     r2, =0x01FFFCFF /* 0x01FFFCFF */
+    str     r2, [r1]
+    
+    /* GPADAT */
+    ldr     r2, =0x01FFFEFF
+    str     r2, [r1, #0x04]
 
     /* MRSRB6 */
+    mov     r1, #0x48000000
+    mov     r2, #0x00000000
+    str     r2, [r1, #0x2C]
+
+    /* GPADAT */
+    mov     r1, #0x56000000
+    ldr     r2, =0x01FFFFFF     
+    str     r2, [r1, #0x04]
+
+    /* MRSRB6 */
+    mov     r1, #0x48000000
     mov     r2, #0x00000030
-    str    r2,=0x4800002C
+    str     r2, [r1, #0x2C]
 
     /* GPACON */
+    mov     r1, #0x56000000 
     mov     r2, #0x01FFFFFF
-    str    r2,=0x56000000
+    str     r2, [r1]
 
     /* End of the unknown */
 #endif
 
-    /* get the high part of our execute address */
+    /* The builds have two potential load addresses, one being from flash, 
-    ldr    r2, =0xffffff00
+     * and the other from some "unknown" location right now the assumption
-    and    r4, pc, r2
+     * 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 bootloader to safe area - 0x31000000 */
+    /* Copy code to 0x30000000 */
-    mov    r5, #0x30000000
+    ldr     r2, = _vectorstart
-    add    r5, r5, #0x1000000
+    ldr     r3, = _iramend
-    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:  */
+    sub     r2, r3, r2           /* length of loader */
     
-start_loc:
+    ldr     r1, =0x30000000     /* copy location */   
-     bl main
     
-#else /* BOOTLOADER */
+    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 */
+
+    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
@@ -276,39 +458,12 @@ start_loc:
     /* 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 */
+    /* Start the main function */
-    .section .vectors,"aw"
+    ldr     pc, =main
-    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 */
+    /* Should never get here, but let's restart in case */
-    .global vectors
+//    b       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
 
 /* 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
+#if defined(BOOTLOADER)
-UIE:
+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 */