as3525*: enable MMU in bootloader

Reserve 1MB of DRAM for loading rockbox and use the rest as BSS
Write sdram setup in assembler and move it to a separate file, together
with MMU init code

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

12 files changed, 200 insertions, 198 deletions

diff --git a/bootloader/sansa_as3525.c b/bootloader/sansa_as3525.c
index 9906991bb4..3eb6159800 100644
--- a/bootloader/sansa_as3525.c
+++ b/bootloader/sansa_as3525.c
@@ -122,11 +122,10 @@ void main(void)
     if(ret < 0)
         error(EBOOTFILE, ret);
 
-    disable_irq(); /* disable irq until we have copied the new vectors */
-
     if (ret == EOK)
     {
         kernel_entry = (void*) loadbuffer;
+        cpucache_invalidate();
         printf("Executing");
         kernel_entry();
         printf("ERR: Failed to boot");
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 4d456d2873..3d9c60de6c 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -441,6 +441,7 @@ target/arm/pnx0101/timer-pnx0101.c
 
 #if CONFIG_CPU == AS3525 || CONFIG_CPU == AS3525v2
 target/arm/as3525/system-as3525.c
+target/arm/as3525/memory-init.S
 target/arm/as3525/kernel-as3525.c
 target/arm/as3525/timer-as3525.c
 #if CONFIG_CPU == AS3525
@@ -455,8 +456,8 @@ target/arm/as3525/power-as3525.c
 target/arm/as3525/usb-as3525.c
 target/arm/as3525/dma-pl081.c
 target/arm/as3525/ascodec-as3525.c
-#ifndef BOOTLOADER
 target/arm/mmu-arm.S
+#ifndef BOOTLOADER
 drivers/generic_i2c.c
 target/arm/adc-as3514.c
 target/arm/as3525/audio-as3525.c
diff --git a/firmware/export/as3525.h b/firmware/export/as3525.h
index 6b0e85f9c5..52f10efb3e 100644
--- a/firmware/export/as3525.h
+++ b/firmware/export/as3525.h
@@ -31,7 +31,6 @@
 #endif
 
 /* Virtual addresses */
-/* Do not apply to the bootloader, which uses physical addresses (no MMU) */
 #define DRAM_ORIG 0x30000000
 #define IRAM_ORIG (DRAM_ORIG + DRAM_SIZE) /* IRAM is mapped just next to DRAM */
 
diff --git a/firmware/export/config.h b/firmware/export/config.h
index 22178e4bd4..de84dc3742 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -748,9 +748,9 @@ Lyre prototype 1 */
     (((CONFIG_CPU == SH7034) && !defined(PLUGIN)) || /* SH1 archos: core only */ \
     defined(CPU_COLDFIRE) || /* Coldfire: core, plugins, codecs */ \
     defined(CPU_PP) ||  /* PortalPlayer: core, plugins, codecs */ \
-    (CONFIG_CPU == AS3525 && MEMORYSIZE > 2) || /* AS3525 +2MB: core, plugins, codecs */ \
-    (CONFIG_CPU == AS3525 && MEMORYSIZE <= 2 && !defined(PLUGIN) && !defined(CODEC)) || /* AS3525 2MB: core only */ \
-    (CONFIG_CPU == AS3525v2 && !defined(PLUGIN) && !defined(CODEC)) || /* AS3525v2: core only */ \
+    (CONFIG_CPU == AS3525 && MEMORYSIZE > 2 && !defined(BOOTLOADER)) || /* AS3525 +2MB: core, plugins, codecs */ \
+    (CONFIG_CPU == AS3525 && MEMORYSIZE <= 2 && !defined(PLUGIN) && !defined(CODEC) && !defined(BOOTLOADER)) || /* AS3525 2MB: core only */ \
+    (CONFIG_CPU == AS3525v2 && !defined(PLUGIN) && !defined(CODEC) && !defined(BOOTLOADER)) || /* AS3525v2: core only */ \
     (CONFIG_CPU == PNX0101) || \
     (CONFIG_CPU == TCC7801) || \
     defined(CPU_S5L870X)) || /* Samsung S5L8700: core, plugins, codecs */ \
diff --git a/firmware/target/arm/as3525/boot.lds b/firmware/target/arm/as3525/boot.lds
index 502e90c22b..2274eea13a 100644
--- a/firmware/target/arm/as3525/boot.lds
+++ b/firmware/target/arm/as3525/boot.lds
@@ -6,39 +6,31 @@ OUTPUT_FORMAT(elf32-littlearm)
 OUTPUT_ARCH(arm)
 STARTUP(target/arm/crt0.o)
 
-#ifdef SANSA_CLIPV2
-#define RAMORIG 0x0             /* DRAM */
-#define RAMSIZE (MEM*0x100000)
-#else
-#define RAMORIG 0x81000000      /* IRAM */
-#define RAMSIZE 0x50000
-#endif
+#define LOAD_SIZE 0x100000  /* Reserve 1MB for loading the firmware */
 
 MEMORY
 {
-    RAM : ORIGIN = RAMORIG, LENGTH = RAMSIZE
+    IRAM : ORIGIN = IRAM_ORIG, LENGTH = IRAM_SIZE
+    DRAM : ORIGIN = DRAM_ORIG + LOAD_SIZE, LENGTH = DRAM_SIZE - LOAD_SIZE - TTB_SIZE
 }
 
 SECTIONS
 {
-  . = RAMORIG;
+  . = IRAM_ORIG;
 
   .text : {
     *(.init.text)
     *(.glue_7)
     *(.glue_7t)
     *(.text*)
-  } > RAM
+    *(.icode)
+  } > IRAM
 
   .data : {
-    *(.icode)
-    *(.irodata)
-    *(.idata)
     *(.data*)
-    *(.ncdata*)
     *(.rodata*)
     _dataend = . ;
-   } > RAM
+   } > IRAM
 
   .stack  (NOLOAD) :
   {
@@ -48,14 +40,12 @@ SECTIONS
      . += 0x2000;
      _stackend = .;
      stackend = .;
-  } > RAM
+  } > IRAM
 
   .bss (NOLOAD) : {
      _edata = .;
      *(.bss*);
-     *(.ibss);
      *(COMMON)
-     *(.ncbss*);
      _end = .;
-   } > RAM
+   } > DRAM
 }
diff --git a/firmware/target/arm/as3525/memory-init.S b/firmware/target/arm/as3525/memory-init.S
new file mode 100644
index 0000000000..fbc11e5c56
--- /dev/null
+++ b/firmware/target/arm/as3525/memory-init.S
@@ -0,0 +1,167 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright © 2010 by Rafaël Carré
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "config.h"
+#include "cpu.h"
+
+#define CACHE_NONE  0
+#define CACHE_ALL   0x0C
+#define UNCACHED_ADDR(a) (a + 0x10000000)
+
+#if defined(SANSA_CLIP) || defined(SANSA_M200V4) || defined(SANSA_C200V2)
+/* 16 bits external bus, low power SDRAM, 16 Mbits = 2 Mbytes */
+#define MEMORY_MODEL 0x21
+
+#elif defined(SANSA_E200V2) || defined(SANSA_FUZE) || defined(SANSA_CLIPV2) \
+    || defined(SANSA_CLIPPLUS) || defined(SANSA_FUZEV2)
+/* 16 bits external bus, high performance SDRAM, 64 Mbits = 8 Mbytes */
+#define MEMORY_MODEL 0x5
+
+#else
+#error "The external memory in your player is unknown"
+#endif
+
+    .global    memory_init
+    .text
+
+memory_init:
+
+#ifdef BOOTLOADER
+
+    ldr     r2, =0xC80F0014 @ CGU_PERI
+    ldr     r1, [r2]
+    orr     r1, r1, #(CGU_EXTMEM_CLOCK_ENABLE|CGU_EXTMEMIF_CLOCK_ENABLE)
+    str     r1, [r2]
+
+    ldr     r3, =0xC6030000 @ MPMC_BASE
+
+    mov     r2, #1          @ enable MPMC
+    str     r2, [r3]        @ MPMC_CONTROL
+    ldr     r2, =0x183      @ SDRAM NOP, all clocks high
+    str     r2, [r3, #0x20] @ MPMC_DYNAMIC_CONTROL
+    ldr     r2, =0x103      @ SDRAM PALL, all clocks high
+    str     r2, [r3, #0x20] @ MPMC_DYNAMIC_CONTROL
+    ldr     r1, =0x138      @ 0x138 * 16 HCLK ticks between SDRAM refresh cycles
+    str     r1, [r3, #0x24] @ MPMC_DYNAMIC_REFRESH
+    mov     r2, #0          @ little endian, HCLK:MPMCCLKOUT[3:0] ratio = 1:1
+    str     r2, [r3, #8]    @ MPMC_CONFIG
+
+    ldr     r2, [r3, #0xfe8]    @ MPMC_PERIPH_ID2
+    tst     r2, #0xf0
+    movne   r2, #1              @ command delayed, clock out not delayed
+    strne   r2, [r3, #0x28]     @ MPMC_DYNAMIC_READ_CONFIG
+
+    mov     r1, #2
+    mov     r0, #5
+    mov     ip, #4
+    mov     r2, #0
+    str     r1, [r3, #0x30]     @ tRP
+    str     ip, [r3, #0x34]     @ tRAS
+    str     r0, [r3, #0x38]     @ tSREX
+    str     r2, [r3, #0x3c]     @ tAPR
+    str     ip, [r3, #0x40]     @ tDAL
+    str     r1, [r3, #0x44]     @ tWR
+    str     r0, [r3, #0x48]     @ tRC
+    str     r0, [r3, #0x4c]     @ tRFC
+    str     r0, [r3, #0x50]     @ tXSR
+    str     r1, [r3, #0x54]     @ tRRD
+    str     r1, [r3, #0x58]     @ tMRD
+    mov     ip, #(MEMORY_MODEL << 7)
+    str     ip, [r3, #0x100]    @ MPMC_DYNAMIC_CONFIG_CONFIG_0
+    orr     r1, r1, #(2<<8)     @ CAS & RAS latency = 2 clock cycle
+    str     r1, [r3, #0x104]    @ MPMC_DYNAMIC_CONFIG_RASCAS_0
+
+    str     r2, [r3, #0x120]    @ MPMC_DYNAMIC_CONFIG_CONFIG_1
+    str     r2, [r3, #0x124]    @ MPMC_DYNAMIC_CONFIG_RASCAS_1
+    str     r2, [r3, #0x140]    @ MPMC_DYNAMIC_CONFIG_CONFIG_2
+    str     r2, [r3, #0x144]    @ MPMC_DYNAMIC_CONFIG_RASCAS_2
+    str     r2, [r3, #0x160]    @ MPMC_DYNAMIC_CONFIG_CONFIG_3
+    str     r2, [r3, #0x164]    @ MPMC_DYNAMIC_CONFIG_RASCAS_3
+
+    mov     r1, #0x82           @ SDRAM MODE, MPMCCLKOUT runs continuously
+    str     r1, [r3, #0x20]     @ MPMC_DYNAMIC_CONTROL
+
+    ldr     r1, =DRAM_ORIG+(0x2300*MEM)
+    ldr     r1, [r1]
+
+    str     r2, [r3, #0x20]     @ MPMC_DYNAMIC_CONTROL= SDRAM NORMAL,
+                                @ MPMCCLKOUT stopped when SDRAM is idle
+
+    ldr     r2, [r3, #0x100]    @ MPMC_DYNAMIC_CONFIG_0
+    orr     r2, r2, #(1<<19)    @ buffer enable
+    str     r2, [r3, #0x100]
+
+#endif /* BOOTLOADER */
+
+    @ XXX: to avoid using the stack, we rely on the fact that:
+    @   - ttb_init
+    @   - map_section
+    @   - enable_mmu
+    @ do not modify ip (r12)
+    mov     ip, lr
+
+    /* Setup MMU */
+
+    bl      ttb_init
+
+    mov     r0, #0                          @ physical address
+    mov     r1, #0                          @ virtual address
+    mov     r2, #0x1000                     @ size (all memory)
+    mov     r3, #CACHE_NONE
+    bl      map_section
+
+    mov     r0, #0                          @ physical address
+    ldr     r1, =IRAM_ORIG                  @ virtual address
+    mov     r2, #1                          @ size : 1MB
+    mov     r3, #CACHE_ALL
+    bl      map_section
+
+    mov     r0, #0                          @ physical address
+    ldr     r1, =UNCACHED_ADDR(IRAM_ORIG)   @ virtual address
+    mov     r2, #1                          @ size : 1MB
+    mov     r3, #CACHE_NONE
+    bl      map_section
+
+    mov     r0, #0x30000000                 @ physical address
+    mov     r1, #DRAM_ORIG                  @ virtual address
+    mov     r2, #MEMORYSIZE                 @ size
+    mov     r3, #CACHE_ALL
+    bl      map_section
+
+    mov     r0, #0x30000000                 @ physical address
+    mov     r1, #UNCACHED_ADDR(DRAM_ORIG)   @ virtual address
+    mov     r2, #MEMORYSIZE                 @ size
+    mov     r3, #CACHE_NONE
+    bl      map_section
+
+    /* map 1st mbyte of RAM at 0x0 to have exception vectors available */
+#ifdef BOOTLOADER
+    mov     r0, #0x81000000                 @ physical address
+#else
+    mov     r0, #0x30000000                 @ physical address
+#endif
+    mov     r1, #0                          @ virtual address
+    mov     r2, #1                          @ size
+    mov     r3, #CACHE_ALL
+    bl      map_section
+
+    bl      enable_mmu
+
+    bx      ip
diff --git a/firmware/target/arm/as3525/sd-as3525.c b/firmware/target/arm/as3525/sd-as3525.c
index 20ad241c02..9e55c16be6 100644
--- a/firmware/target/arm/as3525/sd-as3525.c
+++ b/firmware/target/arm/as3525/sd-as3525.c
@@ -672,9 +672,7 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
     mutex_lock(&sd_mtx);
     sd_enable(true);
-#ifndef BOOTLOADER
     led(true);
-#endif
 
     if (card_info[drive].initialized <= 0)
     {
@@ -824,9 +822,7 @@ sd_transfer_error:
 
 sd_transfer_error_nodma:
 
-#ifndef BOOTLOADER
     led(false);
-#endif
     sd_enable(false);
 
     if (ret)    /* error */
@@ -848,12 +844,10 @@ int sd_write_sectors(IF_MD2(int drive,) unsigned long start, int count,
     return sd_transfer_sectors(IF_MD2(drive,) start, count, (void*)buf, true);
 }
 
-#ifndef BOOTLOADER
 long sd_last_disk_activity(void)
 {
     return last_disk_activity;
 }
-#endif /* !BOOTLOADER */
 
 void sd_enable(bool on)
 {
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 3ba56cf30a..940f183c63 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -30,9 +30,6 @@
 #include "clock-target.h"
 #include "fmradio_i2c.h"
 #include "button-target.h"
-#ifndef BOOTLOADER
-#include "mmu-arm.h"
-#endif
 #include "backlight-target.h"
 
 #define default_interrupt(name) \
@@ -210,81 +207,6 @@ static inline void check_model_variant(void)
 }
 #endif /* SANSA_C200V2*/
 
-#if defined(BOOTLOADER)
-static void sdram_delay(void)
-{
-    int delay = 1024; /* arbitrary */
-    while (delay--) ;
-}
-
-/* Use the same initialization than OF */
-static void sdram_init(void)
-{
-    CGU_PERI |= (CGU_EXTMEM_CLOCK_ENABLE|CGU_EXTMEMIF_CLOCK_ENABLE);
-
-    MPMC_CONTROL = 0x1; /* enable MPMC */
-
-    MPMC_DYNAMIC_CONTROL = 0x183; /* SDRAM NOP, all clocks high */
-    sdram_delay();
-
-    MPMC_DYNAMIC_CONTROL = 0x103; /* SDRAM PALL, all clocks high */
-    sdram_delay();
-
-    MPMC_DYNAMIC_REFRESH = 0x138; /* 0x138 * 16 HCLK ticks between SDRAM refresh cycles */
-
-    MPMC_CONFIG = 0; /* little endian, HCLK:MPMCCLKOUT[3:0] ratio = 1:1 */
-
-    if(MPMC_PERIPH_ID2 & 0xf0)
-        MPMC_DYNAMIC_READ_CONFIG = 0x1; /* command delayed, clock out not delayed */
-
-    /* timings */
-    MPMC_DYNAMIC_tRP    = 2;
-    MPMC_DYNAMIC_tRAS   = 4;
-    MPMC_DYNAMIC_tSREX  = 5;
-    MPMC_DYNAMIC_tAPR   = 0;
-    MPMC_DYNAMIC_tDAL   = 4;
-    MPMC_DYNAMIC_tWR    = 2;
-    MPMC_DYNAMIC_tRC    = 5;
-    MPMC_DYNAMIC_tRFC   = 5;
-    MPMC_DYNAMIC_tXSR   = 5;
-    MPMC_DYNAMIC_tRRD   = 2;
-    MPMC_DYNAMIC_tMRD   = 2;
-
-#if defined(SANSA_CLIP) || defined(SANSA_M200V4) || defined(SANSA_C200V2)
-/* 16 bits external bus, low power SDRAM, 16 Mbits = 2 Mbytes */
-#define MEMORY_MODEL 0x21
-
-#elif defined(SANSA_E200V2) || defined(SANSA_FUZE) || defined(SANSA_CLIPV2) \
-    || defined(SANSA_CLIPPLUS) || defined(SANSA_FUZEV2)
-/* 16 bits external bus, high performance SDRAM, 64 Mbits = 8 Mbytes */
-#define MEMORY_MODEL 0x5
-
-#else
-#error "The external memory in your player is unknown"
-#endif
-
-    MPMC_DYNAMIC_RASCAS_0 = (2<<8)|2; /* CAS & RAS latency = 2 clock cycles */
-    MPMC_DYNAMIC_CONFIG_0 = (MEMORY_MODEL << 7);
-
-    MPMC_DYNAMIC_RASCAS_1 = MPMC_DYNAMIC_CONFIG_1 =
-    MPMC_DYNAMIC_RASCAS_2 = MPMC_DYNAMIC_CONFIG_2 =
-    MPMC_DYNAMIC_RASCAS_3 = MPMC_DYNAMIC_CONFIG_3 = 0;
-
-    MPMC_DYNAMIC_CONTROL = 0x82; /* SDRAM MODE, MPMCCLKOUT runs continuously */
-
-    /* program the SDRAM mode register */
-    /* FIXME: details the exact settings of mode register */
-    asm volatile(
-        "ldr r4, [%0]\n"
-        : : "p"(0x30000000+0x2300*MEM) : "r4");
-
-    /* SDRAM NORMAL, MPMCCLKOUT stopped when SDRAM is idle */
-    MPMC_DYNAMIC_CONTROL = 0x0;
-
-    MPMC_DYNAMIC_CONFIG_0 |= (1<<19); /* buffer enable */
-}
-#endif /* BOOTLOADER */
-
 void system_init(void)
 {
 #if CONFIG_CPU == AS3525v2
@@ -343,9 +265,7 @@ void system_init(void)
 #endif
                   AS3525_PCLK_SEL);
 
-#if defined(BOOTLOADER)
-    sdram_init();
-#elif defined(SANSA_FUZE) || defined(SANSA_CLIP) || defined(SANSA_E200V2)
+#if !defined(BOOTLOADER) && defined(SANSA_FUZE) || defined(SANSA_CLIP) || defined(SANSA_E200V2)
     /* XXX: remove me when we have a new bootloader */
     MPMC_DYNAMIC_CONTROL = 0x0; /* MPMCCLKOUT stops when all SDRAMs are idle */
 #endif  /* BOOTLOADER */
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index 11e89d9f2c..523237ffcd 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -38,11 +38,7 @@
 #define KERNEL_TIMER_FREQ TIMER_FREQ
 #endif
 
-#ifdef BOOTLOADER
-#define AS3525_UNCACHED_ADDR(a) (a)
-#else
 #define AS3525_UNCACHED_ADDR(a) ((typeof(a)) ((uintptr_t)(a) + 0x10000000))
-#endif
 
 #ifdef SANSA_C200V2
 /* 0: Backlight on A5, 1: Backlight on A7 */
diff --git a/firmware/target/arm/as3525/usb-drv-as3525.c b/firmware/target/arm/as3525/usb-drv-as3525.c
index e90bc04aee..0b73713c51 100644
--- a/firmware/target/arm/as3525/usb-drv-as3525.c
+++ b/firmware/target/arm/as3525/usb-drv-as3525.c
@@ -396,8 +396,7 @@ int usb_drv_recv(int ep, void *ptr, int len)
     endpoints[ep][1].rc = -1;
 
     /* remove data buffer from cache */
-    if (!is_bootloader()) /* bootloader is running uncached */
-        invalidate_dcache();
+    invalidate_dcache();
 
     /* DMA setup */
     uc_desc->status    = USB_DMA_DESC_BS_HST_RDY |
@@ -448,8 +447,7 @@ void ep_send(int ep, void *ptr, int len)
     endpoints[ep][0].rc = -1;
 
     /* Make sure data is committed to memory */
-    if (!is_bootloader()) /* bootloader is running uncached */
-        clean_dcache();
+    clean_dcache();
 
     logf("xx%s\n", make_hex(ptr, len));
 
@@ -556,8 +554,7 @@ static void handle_out_ep(int ep)
              /*
               * If parts of the just dmaed range are in cache, dump them now.
               */
-             if (!is_bootloader()) /* bootloader is running uncached */
-                 dump_dcache_range(uc_desc->data_ptr, dma_len);
+             dump_dcache_range(uc_desc->data_ptr, dma_len);
         } else{
              logf("EP%d OUT token, st:%08x frm:%x (no data)\n", ep,
                  dma_mst, dma_frm);
diff --git a/firmware/target/arm/as3525/usb-drv-as3525.h b/firmware/target/arm/as3525/usb-drv-as3525.h
index f579638cfb..3a434170d7 100644
--- a/firmware/target/arm/as3525/usb-drv-as3525.h
+++ b/firmware/target/arm/as3525/usb-drv-as3525.h
@@ -23,12 +23,6 @@
 
 #include "as3525.h"
 
-#ifdef BOOTLOADER
-#define is_bootloader() 1
-#else
-#define is_bootloader() 0
-#endif /* BOOTLOADER */
-
 #define USB_NUM_EPS         4
 
 typedef struct {
diff --git a/firmware/target/arm/crt0.S b/firmware/target/arm/crt0.S
index 6def9a1a88..f52056617a 100644
--- a/firmware/target/arm/crt0.S
+++ b/firmware/target/arm/crt0.S
@@ -26,23 +26,14 @@
     .global    start
 start:
     /* Exception vectors */
-    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
+    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
 
 _vectorsend:
 
@@ -51,56 +42,11 @@ _vectorsend:
 newstart:
     msr     cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */
 
-#if (CONFIG_CPU==AS3525 || CONFIG_CPU==AS3525v2) && !defined(BOOTLOADER)
-
-#define CACHE_NONE  0
-#define CACHE_ALL   0x0C
-#define UNCACHED_ADDR(a) (a + 0x10000000)
-
-    /* Setup MMU : has to be done before accessing IRAM ! */
-
-    bl      ttb_init
-
-    mov     r0, #0                          @ physical address
-    mov     r1, #0                          @ virtual address
-    mov     r2, #0x1000                     @ size (all memory)
-    mov     r3, #CACHE_NONE
-    bl      map_section
-
-    mov     r0, #0                          @ physical address
-    ldr     r1, =IRAM_ORIG                  @ virtual address
-    mov     r2, #1                          @ size : 1MB
-    mov     r3, #CACHE_ALL
-    bl      map_section
-
-    mov     r0, #0                          @ physical address
-    ldr     r1, =UNCACHED_ADDR(IRAM_ORIG)   @ virtual address
-    mov     r2, #1                          @ size : 1MB
-    mov     r3, #CACHE_NONE
-    bl      map_section
-
-    mov     r0, #0x30000000                 @ physical address
-    mov     r1, #DRAM_ORIG                  @ virtual address
-    mov     r2, #MEMORYSIZE                 @ size
-    mov     r3, #CACHE_ALL
-    bl      map_section
-
-    mov     r0, #0x30000000                 @ physical address
-    mov     r1, #UNCACHED_ADDR(DRAM_ORIG)   @ virtual address
-    mov     r2, #MEMORYSIZE                 @ size
-    mov     r3, #CACHE_NONE
-    bl      map_section
-
-    /* map 1st mbyte of DRAM at 0x0 to have exception vectors available */
-
-    mov     r0, #0x30000000                 @ physical address
-    mov     r1, #0                          @ virtual address
-    mov     r2, #1                          @ size
-    mov     r3, #CACHE_ALL
-    bl      map_section
-
-    bl      enable_mmu
+#if CONFIG_CPU == AS3525 || CONFIG_CPU == AS3525v2
+    bl      memory_init
+#endif
 
+#ifdef USE_IRAM
     /* Zero out IBSS */
     ldr     r2, =_iedata
     ldr     r3, =_iend
@@ -120,7 +66,6 @@ newstart:
     ldrhi   r5, [r2], #4
     strhi   r5, [r3], #4
     bhi     1b
-
 #endif
 
 #ifdef HAVE_INIT_ATTR
@@ -173,8 +118,8 @@ newstart:
 
     /* Switch back to supervisor mode */
     msr     cpsr_c, #0xd3
-    bl      main
-
+    ldr     ip, =main       @ make sure we are using the virtual address
+    bx      ip
 
 /* All illegal exceptions call into UIE with exception address as first
  * parameter. This is calculated differently depending on which exception