PXA: Fix reloc, Push lowlevel init into C code

Firstly, this fixes relocation issues. I had to use part of Dcache as RAM for a
while. I also moved around the lowlevel init code. It turned out so most of the
lowlevel init code ended in cpu.c (and eventually was rewritten into C).

This will also allow easier operation with FDT, multi-CPU-model support etc. in
later releases.

NOTE: This breaks most of the PXA boards (actually, the reloc stuff did already,
this only finishes the doom).

Signed-off-by: Marek Vasut <marek.vasut@gmail.com>

2 files changed, 359 insertions, 214 deletions

diff --git a/arch/arm/cpu/pxa/cpu.c b/arch/arm/cpu/pxa/cpu.c
index 330d013c6d..3ea34582e7 100644
--- a/arch/arm/cpu/pxa/cpu.c
+++ b/arch/arm/cpu/pxa/cpu.c
@@ -30,11 +30,11 @@
  * CPU specific code
  */
 
-#include <asm/arch/pxa-regs.h>
 #include <asm/io.h>
 #include <asm/system.h>
 #include <command.h>
 #include <common.h>
+#include <asm/arch/pxa-regs.h>
 
 static void cache_flush(void);
 
@@ -91,3 +91,230 @@ void set_GPIO_mode(int gpio_mode)
 	writel(val, GAFR(gpio));
 }
 #endif /* CONFIG_CPU_MONAHANS */
+
+void pxa_wait_ticks(int ticks)
+{
+	writel(0, OSCR);
+	while (readl(OSCR) < ticks)
+		asm volatile("":::"memory");
+}
+
+inline void writelrb(uint32_t val, uint32_t addr)
+{
+	writel(val, addr);
+	asm volatile("":::"memory");
+	readl(addr);
+	asm volatile("":::"memory");
+}
+
+void pxa_dram_init(void)
+{
+	uint32_t tmp;
+	int i;
+	/*
+	 * 1) Initialize Asynchronous static memory controller
+	 */
+
+	writelrb(CONFIG_SYS_MSC0_VAL, MSC0);
+	writelrb(CONFIG_SYS_MSC1_VAL, MSC1);
+	writelrb(CONFIG_SYS_MSC2_VAL, MSC2);
+	/*
+	 * 2) Initialize Card Interface
+	 */
+
+	/* MECR: Memory Expansion Card Register */
+	writelrb(CONFIG_SYS_MECR_VAL, MECR);
+	/* MCMEM0: Card Interface slot 0 timing */
+	writelrb(CONFIG_SYS_MCMEM0_VAL, MCMEM0);
+	/* MCMEM1: Card Interface slot 1 timing */
+	writelrb(CONFIG_SYS_MCMEM1_VAL, MCMEM1);
+	/* MCATT0: Card Interface Attribute Space Timing, slot 0 */
+	writelrb(CONFIG_SYS_MCATT0_VAL, MCATT0);
+	/* MCATT1: Card Interface Attribute Space Timing, slot 1 */
+	writelrb(CONFIG_SYS_MCATT1_VAL, MCATT1);
+	/* MCIO0: Card Interface I/O Space Timing, slot 0 */
+	writelrb(CONFIG_SYS_MCIO0_VAL, MCIO0);
+	/* MCIO1: Card Interface I/O Space Timing, slot 1 */
+	writelrb(CONFIG_SYS_MCIO1_VAL, MCIO1);
+
+	/*
+	 * 3) Configure Fly-By DMA register
+	 */
+
+	writelrb(CONFIG_SYS_FLYCNFG_VAL, FLYCNFG);
+
+	/*
+	 * 4) Initialize Timing for Sync Memory (SDCLK0)
+	 */
+
+	/*
+	 * Before accessing MDREFR we need a valid DRI field, so we set
+	 * this to power on defaults + DRI field.
+	 */
+
+	/* Read current MDREFR config and zero out DRI */
+	tmp = readl(MDREFR) & ~0xfff;
+	/* Add user-specified DRI */
+	tmp |= CONFIG_SYS_MDREFR_VAL & 0xfff;
+	/* Configure important bits */
+	tmp |= MDREFR_K0RUN | MDREFR_SLFRSH;
+	tmp &= ~(MDREFR_APD | MDREFR_E1PIN);
+
+	/* Write MDREFR back */
+	writelrb(tmp, MDREFR);
+
+	/*
+	 * 5) Initialize Synchronous Static Memory (Flash/Peripherals)
+	 */
+
+	/* Initialize SXCNFG register. Assert the enable bits.
+	 *
+	 * Write SXMRS to cause an MRS command to all enabled banks of
+	 * synchronous static memory. Note that SXLCR need not be written
+	 * at this time.
+	 */
+	writelrb(CONFIG_SYS_SXCNFG_VAL, SXCNFG);
+
+	/*
+	 * 6) Initialize SDRAM
+	 */
+
+	writelrb(CONFIG_SYS_MDREFR_VAL & ~MDREFR_SLFRSH, MDREFR);
+	writelrb(CONFIG_SYS_MDREFR_VAL | MDREFR_E1PIN, MDREFR);
+
+	/*
+	 * 7) Write MDCNFG with MDCNFG:DEx deasserted (set to 0), to configure
+	 *    but not enable each SDRAM partition pair.
+	 */
+
+	writelrb(CONFIG_SYS_MDCNFG_VAL &
+		~(MDCNFG_DE0 | MDCNFG_DE1 | MDCNFG_DE2 | MDCNFG_DE3), MDCNFG);
+	/* Wait for the clock to the SDRAMs to stabilize, 100..200 usec. */
+	pxa_wait_ticks(0x300);
+
+	/*
+	 * 8) Trigger a number (usually 8) refresh cycles by attempting
+	 *    non-burst read or write accesses to disabled SDRAM, as commonly
+	 *    specified in the power up sequence documented in SDRAM data
+	 *    sheets. The address(es) used for this purpose must not be
+	 *    cacheable.
+	 */
+	for (i = 9; i >= 0; i--) {
+		writel(i, 0xa0000000);
+		asm volatile("":::"memory");
+	}
+	/*
+	 * 9) Write MDCNFG with enable bits asserted (MDCNFG:DEx set to 1).
+	 */
+
+	tmp = CONFIG_SYS_MDCNFG_VAL &
+		(MDCNFG_DE0 | MDCNFG_DE1 | MDCNFG_DE2 | MDCNFG_DE3);
+	tmp |= readl(MDCNFG);
+	writelrb(tmp, MDCNFG);
+
+	/*
+	 * 10) Write MDMRS.
+	 */
+
+	writelrb(CONFIG_SYS_MDMRS_VAL, MDMRS);
+
+	/*
+	 * 11) Enable APD
+	 */
+
+	if (CONFIG_SYS_MDREFR_VAL & MDREFR_APD) {
+		tmp = readl(MDREFR);
+		tmp |= MDREFR_APD;
+		writelrb(tmp, MDREFR);
+	}
+}
+
+void pxa_gpio_setup(void)
+{
+	writel(CONFIG_SYS_GPSR0_VAL, GPSR0);
+	writel(CONFIG_SYS_GPSR1_VAL, GPSR1);
+	writel(CONFIG_SYS_GPSR2_VAL, GPSR2);
+#if defined(CONFIG_PXA27X) || defined(CONFIG_CPU_MONAHANS)
+	writel(CONFIG_SYS_GPSR3_VAL, GPSR3);
+#endif
+
+	writel(CONFIG_SYS_GPCR0_VAL, GPCR0);
+	writel(CONFIG_SYS_GPCR1_VAL, GPCR1);
+	writel(CONFIG_SYS_GPCR2_VAL, GPCR2);
+#if defined(CONFIG_PXA27X) || defined(CONFIG_CPU_MONAHANS)
+	writel(CONFIG_SYS_GPCR3_VAL, GPCR3);
+#endif
+
+	writel(CONFIG_SYS_GPDR0_VAL, GPDR0);
+	writel(CONFIG_SYS_GPDR1_VAL, GPDR1);
+	writel(CONFIG_SYS_GPDR2_VAL, GPDR2);
+#if defined(CONFIG_PXA27X) || defined(CONFIG_CPU_MONAHANS)
+	writel(CONFIG_SYS_GPDR3_VAL, GPDR3);
+#endif
+
+	writel(CONFIG_SYS_GAFR0_L_VAL, GAFR0_L);
+	writel(CONFIG_SYS_GAFR0_U_VAL, GAFR0_U);
+	writel(CONFIG_SYS_GAFR1_L_VAL, GAFR1_L);
+	writel(CONFIG_SYS_GAFR1_U_VAL, GAFR1_U);
+	writel(CONFIG_SYS_GAFR2_L_VAL, GAFR2_L);
+	writel(CONFIG_SYS_GAFR2_U_VAL, GAFR2_U);
+#if defined(CONFIG_PXA27X) || defined(CONFIG_CPU_MONAHANS)
+	writel(CONFIG_SYS_GAFR3_L_VAL, GAFR3_L);
+	writel(CONFIG_SYS_GAFR3_U_VAL, GAFR3_U);
+#endif
+
+	writel(CONFIG_SYS_PSSR_VAL, PSSR);
+}
+
+void pxa_interrupt_setup(void)
+{
+	writel(0, ICLR);
+	writel(0, ICMR);
+#if defined(CONFIG_PXA27X) || defined(CONFIG_CPU_MONAHANS)
+	writel(0, ICLR2);
+	writel(0, ICMR2);
+#endif
+}
+
+void pxa_clock_setup(void)
+{
+#ifndef CONFIG_CPU_MONAHANS
+	writel(CONFIG_SYS_CKEN, CKEN);
+	writel(CONFIG_SYS_CCCR, CCCR);
+	asm volatile("mcr	p14, 0, %0, c6, c0, 0"::"r"(2));
+#else
+/* Set CKENA/CKENB/ACCR for MH */
+#endif
+
+	/* enable the 32Khz oscillator for RTC and PowerManager */
+	writel(OSCC_OON, OSCC);
+	while(!(readl(OSCC) & OSCC_OOK))
+		asm volatile("":::"memory");
+}
+
+void pxa_wakeup(void)
+{
+	uint32_t rcsr;
+
+	rcsr = readl(RCSR);
+	writel(rcsr & (RCSR_GPR | RCSR_SMR | RCSR_WDR | RCSR_HWR), RCSR);
+
+	/* Wakeup */
+	if (rcsr & RCSR_SMR) {
+		writel(PSSR_PH, PSSR);
+		pxa_dram_init();
+		icache_disable();
+		dcache_disable();
+		asm volatile("mov	pc, %0"::"r"(readl(PSSR)));
+	}
+}
+
+int arch_cpu_init(void)
+{
+	pxa_gpio_setup();
+//	pxa_wait_ticks(0x8000);
+	pxa_wakeup();
+	pxa_interrupt_setup();
+	pxa_clock_setup();
+	return 0;
+}
diff --git a/arch/arm/cpu/pxa/start.S b/arch/arm/cpu/pxa/start.S
index 9c5023bae4..684e44e838 100644
--- a/arch/arm/cpu/pxa/start.S
+++ b/arch/arm/cpu/pxa/start.S
@@ -31,6 +31,14 @@
 #include <config.h>
 #include <version.h>
 #include <asm/arch/pxa-regs.h>
+#include <asm/arch/macro.h>
+
+/* takes care the CP15 update has taken place */
+.macro CPWAIT reg
+mrc  p15,0,\reg,c2,c0,0
+mov  \reg,\reg
+sub  pc,pc,#4
+.endm
 
 .globl _start
 _start: b	reset
@@ -86,11 +94,9 @@ _fiq:			.word fiq
 _TEXT_BASE:
 	.word	CONFIG_SYS_TEXT_BASE
 
-#if defined(CONFIG_SYS_ARM_WITHOUT_RELOC)
 .globl _armboot_start
 _armboot_start:
 	.word _start
-#endif
 
 /*
  * These are defined in the board-specific linker script.
@@ -115,7 +121,7 @@ FIQ_STACK_START:
 	.word 0x0badc0de
 #endif /* CONFIG_USE_IRQ */
 
-#if !defined(CONFIG_SYS_ARM_WITHOUT_RELOC)
+#ifndef CONFIG_PRELOADER
 /* IRQ stack memory (calculated at run-time) + 8 bytes */
 .globl IRQ_STACK_START_IN
 IRQ_STACK_START_IN:
@@ -159,12 +165,84 @@ reset:
 	msr	cpsr,r0
 
 	/*
-	 * we do sys-critical inits only at reboot,
-	 * not when booting from ram!
+	 * Enable MMU to use DCache as DRAM
 	 */
-#ifndef CONFIG_SKIP_LOWLEVEL_INIT
-	bl	cpu_init_crit
-#endif
+	/* Domain access -- enable for all CPs */
+	ldr	r0, =0x0000ffff
+	mcr	p15, 0, r0, c3, c0, 0
+
+	/* Point TTBR to MMU table */
+	ldr	r0, =mmu_table
+	adr	r2, _start
+	orr	r0, r2
+	mcr	p15, 0, r0, c2, c0, 0
+
+/* !!! Hereby, check if the code is running from SRAM !!! */
+/* If the code is running from SRAM, alias SRAM to 0x0 to simulate NOR. The code
+ * is linked to 0x0 too, so this makes things easier. */
+	cmp	r2, #0x5c000000
+
+	ldreq	r1, [r0]
+	orreq	r1, r2
+	streq	r1, [r0]
+
+	/* Kick in MMU, ICache, DCache, BTB */
+	mrc	p15, 0, r0, c1, c0, 0
+	bic	r0, #0x1b00
+	bic	r0, #0x0087
+	orr	r0, #0x1800
+	orr	r0, #0x0005
+	mcr	p15, 0, r0, c1, c0, 0
+	CPWAIT	r0
+
+	/* Unlock Icache, Dcache */
+	mcr	p15, 0, r0, c9, c1, 1
+	mcr	p15, 0, r0, c9, c2, 1
+
+	/* Flush Icache, Dcache, BTB */
+	mcr	p15, 0, r0, c7, c7, 0
+
+	/* Unlock I-TLB, D-TLB */
+	mcr	p15, 0, r0, c10, c4, 1
+	mcr	p15, 0, r0, c10, c8, 1
+
+	/* Flush TLB */
+	mcr	p15, 0, r0, c8, c7, 0
+	/* Allocate 4096 bytes of Dcache as RAM */
+
+	/* Drain pending loads and stores */
+	mcr	p15, 0, r0, c7, c10, 4
+
+	mov	r4, #0x00
+	mov	r5, #0x00
+	mov	r2, #0x01
+	mcr	p15, 0, r0, c9, c2, 0
+	CPWAIT	r0
+
+	/* 128 lines reserved (128 x 32bytes = 4096 bytes total) */
+	mov	r0, #128
+	mov	r1, #0xa0000000
+alloc:
+	mcr	p15, 0, r1, c7, c2, 5
+	/* Drain pending loads and stores */
+	mcr	p15, 0, r0, c7, c10, 4
+	strd	r4, [r1], #8
+	strd	r4, [r1], #8
+	strd	r4, [r1], #8
+	strd	r4, [r1], #8
+	subs	r0, #0x01
+	bne	alloc
+	/* Drain pending loads and stores */
+	mcr	p15, 0, r0, c7, c10, 4
+	mov	r2, #0x00
+	mcr	p15, 0, r2, c9, c2, 0
+	CPWAIT	r0
+
+	/* Jump to 0x0 ( + offset) if running from SRAM */
+	adr	r0, zerojmp
+	bic	r0, #0x5c000000
+	mov	pc, r0
+zerojmp:
 
 /* Set stackpointer in internal RAM to call board_init_f */
 call_board_init_f:
@@ -201,11 +279,13 @@ stack_setup:
 	beq	clear_bss
 
 #ifndef CONFIG_SKIP_RELOCATE_UBOOT
+	stmfd sp!, {r0-r12}
 copy_loop:
-	ldmia	r0!, {r9-r10}		/* copy from source address [r0]    */
-	stmia	r6!, {r9-r10}		/* copy to   target address [r1]    */
+	ldmia	r0!, {r3-r5, r7-r11}	/* copy from source address [r0]    */
+	stmia	r6!, {r3-r5, r7-r11}	/* copy to   target address [r1]    */
 	cmp	r0, r2			/* until source end address [r2]    */
 	blo	copy_loop
+	ldmfd sp!, {r0-r12}
 
 #ifndef CONFIG_PRELOADER
 	/* fix got entries */
@@ -274,218 +354,28 @@ _board_init_r: .word board_init_r
 
 /****************************************************************************/
 /*									    */
-/* the actual reset code						    */
+/* the actual reset code for OneNAND IPL				    */
 /*									    */
 /****************************************************************************/
 
+#ifndef	CONFIG_PXA27X
+#error OneNAND IPL is not supported on PXA25x and 26x due to lack of SRAM
+#endif
+
 reset:
-	mrs	r0,cpsr			/* set the CPU to SVC32 mode	    */
-	bic	r0,r0,#0x1f		/* (superviser mode, M=10011)	    */
+	/* Set CPU to SVC32 mode */
+	mrs	r0,cpsr
+	bic	r0,r0,#0x1f
 	orr	r0,r0,#0x13
 	msr	cpsr,r0
 
-	/*
-	 * we do sys-critical inits only at reboot,
-	 * not when booting from RAM!
-	 */
-#ifndef CONFIG_SKIP_LOWLEVEL_INIT
-	bl	cpu_init_crit		/* we do sys-critical inits	    */
-#endif /* !CONFIG_SKIP_LOWLEVEL_INIT */
-
-#ifndef CONFIG_SKIP_RELOCATE_UBOOT
-relocate:				/* relocate U-Boot to RAM	    */
-	adr	r0, _start		/* r0 <- current position of code   */
-	ldr	r1, _TEXT_BASE		/* test if we run from flash or RAM */
-#ifndef	CONFIG_PRELOADER
-	cmp	r0, r1			/* don't reloc during debug	    */
-	beq	stack_setup
-#endif
-
-	ldr	r2, _armboot_start
-	ldr	r3, _bss_start
-	sub	r2, r3, r2		/* r2 <- size of armboot	    */
-	add	r2, r0, r2		/* r2 <- source end address	    */
-
-copy_loop:
-	ldmia	r0!, {r3-r10}		/* copy from source address [r0]    */
-	stmia	r1!, {r3-r10}		/* copy to   target address [r1]    */
-	cmp	r0, r2			/* until source end address [r2]    */
-	blo	copy_loop
-#endif /* !CONFIG_SKIP_RELOCATE_UBOOT */
+	/* Point stack at the end of SRAM and leave 32 words for abort-stack */
+	ldr	sp, =0x5c03ff80
 
-	/* Set up the stack						    */
-stack_setup:
-	ldr	r0, _TEXT_BASE		/* upper 128 KiB: relocated uboot   */
-#ifdef CONFIG_PRELOADER
-	sub	sp, r0, #128		/* leave 32 words for abort-stack   */
-#else
-	sub	r0, r0, #CONFIG_SYS_MALLOC_LEN /* malloc area		    */
-	sub	r0, r0, #CONFIG_SYS_GBL_DATA_SIZE /* bdinfo		    */
-#ifdef CONFIG_USE_IRQ
-	sub	r0, r0, #(CONFIG_STACKSIZE_IRQ+CONFIG_STACKSIZE_FIQ)
-#endif /* CONFIG_USE_IRQ */
-	sub	sp, r0, #12		/* leave 3 words for abort-stack    */
-	bic	sp, sp, #7		/* 8-byte alignment for ABI compliance */
-#endif
+	/* Start OneNAND IPL */
+	ldr	pc, =start_oneboot
 
-clear_bss:
-	ldr	r0, _bss_start		/* find start of bss segment	    */
-	ldr	r1, _bss_end		/* stop here			    */
-	mov	r2, #0x00000000		/* clear			    */
-
-#ifndef CONFIG_PRELOADER
-clbss_l:str	r2, [r0]		/* clear loop...		    */
-	add	r0, r0, #4
-	cmp	r0, r1
-	blo	clbss_l
-#endif
-
-	ldr	pc, _start_armboot
-
-#ifdef CONFIG_ONENAND_IPL
-_start_armboot: .word start_oneboot
-#else
-_start_armboot: .word start_armboot
-#endif
-#endif /* #if !defined(CONFIG_SYS_ARM_WITHOUT_RELOC) */
-
-/****************************************************************************/
-/*									    */
-/* CPU_init_critical registers						    */
-/*									    */
-/* - setup important registers						    */
-/* - setup memory timing						    */
-/*									    */
-/****************************************************************************/
-/* mk@tbd: Fix this! */
-#undef RCSR
-#undef ICMR
-#undef OSMR3
-#undef OSCR
-#undef OWER
-#undef OIER
-#undef CCCR
-
-/* Interrupt-Controller base address					    */
-IC_BASE:	   .word	   0x40d00000
-#define ICMR	0x04
-
-/* Reset-Controller */
-RST_BASE:	.word	0x40f00030
-#define RCSR	0x00
-
-/* Operating System Timer */
-OSTIMER_BASE:	.word	0x40a00000
-#define OSMR3	0x0C
-#define OSCR	0x10
-#define OWER	0x18
-#define OIER	0x1C
-
-/* Clock Manager Registers						    */
-#ifdef CONFIG_CPU_MONAHANS
-# ifndef CONFIG_SYS_MONAHANS_RUN_MODE_OSC_RATIO
-#  error "You have to define CONFIG_SYS_MONAHANS_RUN_MODE_OSC_RATIO!!"
-# endif /* !CONFIG_SYS_MONAHANS_RUN_MODE_OSC_RATIO */
-# ifndef CONFIG_SYS_MONAHANS_TURBO_RUN_MODE_RATIO
-#  define CONFIG_SYS_MONAHANS_TURBO_RUN_MODE_RATIO 0x1
-# endif /* !CONFIG_SYS_MONAHANS_TURBO_RUN_MODE_RATIO */
-#else /* !CONFIG_CPU_MONAHANS */
-#ifdef CONFIG_SYS_CPUSPEED
-CC_BASE:	.word	0x41300000
-#define CCCR	0x00
-cpuspeed:	.word	CONFIG_SYS_CPUSPEED
-#else /* !CONFIG_SYS_CPUSPEED */
-#error "You have to define CONFIG_SYS_CPUSPEED!!"
-#endif /* CONFIG_SYS_CPUSPEED */
-#endif /* CONFIG_CPU_MONAHANS */
-
-	/* takes care the CP15 update has taken place */
-	.macro CPWAIT reg
-	mrc  p15,0,\reg,c2,c0,0
-	mov  \reg,\reg
-	sub  pc,pc,#4
-	.endm
-
-cpu_init_crit:
-
-	/* mask all IRQs						    */
-#ifndef CONFIG_CPU_MONAHANS
-	ldr	r0, IC_BASE
-	mov	r1, #0x00
-	str	r1, [r0, #ICMR]
-#else /* CONFIG_CPU_MONAHANS */
-	/* Step 1 - Enable CP6 permission */
-	mrc	p15, 0, r1, c15, c1, 0	@ read CPAR
-	orr	r1, r1, #0x40
-		mcr	p15, 0, r1, c15, c1, 0
-	CPWAIT	r1
-
-	/* Step 2 - Mask ICMR & ICMR2 */
-	mov	r1, #0
-	mcr	p6, 0, r1, c1, c0, 0	@ ICMR
-	mcr	p6, 0, r1, c7, c0, 0	@ ICMR2
-
-	/* turn off all clocks but the ones we will definitly require */
-	ldr	r1, =CKENA
-	ldr	r2, =(CKENA_22_FFUART | CKENA_10_SRAM | CKENA_9_SMC | CKENA_8_DMC)
-	str	r2, [r1]
-	ldr	r1, =CKENB
-	ldr	r2, =(CKENB_6_IRQ)
-	str	r2, [r1]
-#endif /* !CONFIG_CPU_MONAHANS */
-
-	/* set clock speed */
-#ifdef CONFIG_CPU_MONAHANS
-	ldr	r0, =ACCR
-	ldr	r1, =(((CONFIG_SYS_MONAHANS_TURBO_RUN_MODE_RATIO<<8) & ACCR_XN_MASK) | (CONFIG_SYS_MONAHANS_RUN_MODE_OSC_RATIO & ACCR_XL_MASK))
-	str	r1, [r0]
-#else /* !CONFIG_CPU_MONAHANS */
-#ifdef CONFIG_SYS_CPUSPEED
-	ldr	r0, CC_BASE
-	ldr	r1, cpuspeed
-	str	r1, [r0, #CCCR]
-	mov	r0, #2
-	mcr	p14, 0, r0, c6, c0, 0
-
-setspeed_done:
-
-#endif /* CONFIG_SYS_CPUSPEED */
-#endif /* CONFIG_CPU_MONAHANS */
-
-	/*
-	 * before relocating, we have to setup RAM timing
-	 * because memory timing is board-dependend, you will
-	 * find a lowlevel_init.S in your board directory.
-	 */
-	mov	ip,	lr
-	bl	lowlevel_init
-	mov	lr,	ip
-
-	/* Memory interfaces are working. Disable MMU and enable I-cache.   */
-	/* mk: hmm, this is not in the monahans docs, leave it now but
-	 *     check here if it doesn't work :-) */
-
-	ldr	r0, =0x2001		/* enable access to all coproc.	    */
-	mcr	p15, 0, r0, c15, c1, 0
-	CPWAIT r0
-
-	mcr	p15, 0, r0, c7, c10, 4	/* drain the write & fill buffers   */
-	CPWAIT r0
-
-	mcr	p15, 0, r0, c7, c7, 0	/* flush Icache, Dcache and BTB	    */
-	CPWAIT r0
-
-	mcr	p15, 0, r0, c8, c7, 0	/* flush instuction and data TLBs   */
-	CPWAIT r0
-
-	/* Enable the Icache						    */
-/*
-	mrc	p15, 0, r0, c1, c0, 0
-	orr	r0, r0, #0x1800
-	mcr	p15, 0, r0, c1, c0, 0
-	CPWAIT
-*/
-	mov	pc, lr
+#endif /* #if !defined(CONFIG_ONENAND_IPL) */
 
 #ifndef CONFIG_PRELOADER
 /****************************************************************************/
@@ -676,6 +566,12 @@ fiq:
 /* perform a watchdog timeout for a soft reset.				    */
 /*									    */
 /****************************************************************************/
+/* Operating System Timer */
+OSTIMER_BASE:	.word	0x40a00000
+#define OSMR3	0x0C
+#define OSCR	0x10
+#define OWER	0x18
+#define OIER	0x1C
 
 	.align	5
 .globl reset_cpu
@@ -703,3 +599,25 @@ reset_cpu:
 reset_endless:
 
 	b	reset_endless
+
+#ifndef CONFIG_PRELOADER
+.section .mmudata, "a"
+	.align	14
+	.globl	mmu_table
+mmu_table:
+	/* 0x00000000 - 0xa0000000 : 1:1, uncached mapping */
+	.set	__base, 0
+	.rept	0xa00
+	.word	(__base << 20) | 0xc12
+	.set	__base, __base + 1
+	.endr
+
+	/* 0xa0000000 - 0xa0100000 : 1:1, cached mapping */
+	.word	(0xa00 << 20) | 0x1c1e
+
+	.set	__base, 0xa01
+	.rept	0x1000 - 0xa01
+	.word	(__base << 20) | 0xc12
+	.set	__base, __base + 1
+	.endr
+#endif