* Cleanup lowboot code for MPC5200

[platform/kernel/u-boot.git] / cpu / mpc5xxx / start.S

diff --git a/cpu/mpc5xxx/start.S b/cpu/mpc5xxx/start.S
index 4145442..a001e1c 100644 (file)
--- a/cpu/mpc5xxx/start.S
+++ b/cpu/mpc5xxx/start.S
@@ -103,7 +103,42 @@ boot_cold:
 boot_warm:
        mfmsr   r5                      /* save msr contents            */
 
-#if defined(CFG_DEFAULT_MBAR)
+#if defined(CFG_LOWBOOT)
+       lis     r4, CFG_DEFAULT_MBAR@h
+       lis     r3,     0x0000FF00@h
+       ori     r3, r3, 0x0000FF00@l
+       stw     r3, 0x4(r4)
+       lis     r3,     0x0000FFFF@h
+       ori     r3, r3, 0x0000FFFF@l
+       stw     r3, 0x8(r4)
+       lis     r3,     0x00047800@h
+       ori     r3, r3, 0x00047800@l
+       stw     r3, 0x300(r4)
+       lis     r3,     0x02010000@h
+       ori     r3, r3, 0x02010000@l
+       stw     r3, 0x54(r4)
+
+       lis     r3,     lowboot_reentry@h
+       ori     r3, r3, lowboot_reentry@l
+       mtlr    r3
+       blr                             /* jump to flash based address */
+
+lowboot_reentry:
+       lis     r3,     0x0000FF00@h
+       ori     r3, r3, 0x0000FF00@l
+       stw     r3, 0x4c(r4)
+       lis     r3,     0x0000FFFF@h
+       ori     r3, r3, 0x0000FFFF@l
+       stw     r3, 0x50(r4)
+       lis     r3,     0x00047800@h
+       ori     r3, r3, 0x00047800@l
+       stw     r3, 0x300(r4)
+       lis     r3,     0x02000001@h
+       ori     r3, r3, 0x02000001@l
+       stw     r3, 0x54(r4)
+#endif /* CFG_LOWBOOT */
+
+#if defined(CFG_DEFAULT_MBAR) && !defined(CFG_RAMBOOT)
        lis     r3, CFG_MBAR@h
        ori     r3, r3, CFG_MBAR@l
 #if defined(CONFIG_MPC5200)
@@ -208,75 +243,7 @@ ProgramCheck:
 
        STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
        STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
-
-       . = 0xc00
-/*
- * r0 - SYSCALL number
- * r3-... arguments
- */
-SystemCall:
-       addis   r11,r0,0                /* get functions table addr */
-       ori     r11,r11,0               /* Note: this code is patched in trap_init */
-       addis   r12,r0,0                /* get number of functions */
-       ori     r12,r12,0
-
-       cmplw   0, r0, r12
-       bge     1f
-
-       rlwinm  r0,r0,2,0,31            /* fn_addr = fn_tbl[r0] */
-       add     r11,r11,r0
-       lwz     r11,0(r11)
-
-       li      r20,0xd00-4             /* Get stack pointer */
-       lwz     r12,0(r20)
-       subi    r12,r12,12              /* Adjust stack pointer */
-       li      r0,0xc00+_end_back-SystemCall
-       cmplw   0, r0, r12              /* Check stack overflow */
-       bgt     1f
-       stw     r12,0(r20)
-
-       mflr    r0
-       stw     r0,0(r12)
-       mfspr   r0,SRR0
-       stw     r0,4(r12)
-       mfspr   r0,SRR1
-       stw     r0,8(r12)
-
-       li      r12,0xc00+_back-SystemCall
-       mtlr    r12
-       mtspr   SRR0,r11
-
-1:     SYNC
-       rfi
-
-_back:
-
-       mfmsr   r11                     /* Disable interrupts */
-       li      r12,0
-       ori     r12,r12,MSR_EE
-       andc    r11,r11,r12
-       SYNC                            /* Some chip revs need this... */
-       mtmsr   r11
-       SYNC
-
-       li      r12,0xd00-4             /* restore regs */
-       lwz     r12,0(r12)
-
-       lwz     r11,0(r12)
-       mtlr    r11
-       lwz     r11,4(r12)
-       mtspr   SRR0,r11
-       lwz     r11,8(r12)
-       mtspr   SRR1,r11
-
-       addi    r12,r12,12              /* Adjust stack pointer */
-       li      r20,0xd00-4
-       stw     r12,0(r20)
-
-       SYNC
-       rfi
-_end_back:
-
+       STD_EXCEPTION(0xc00, SystemCall, UnknownException)
        STD_EXCEPTION(0xd00, SingleStep, UnknownException)
 
        STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
@@ -450,6 +417,14 @@ init_5xxx_core:
        mtspr   DBAT2L, r0
        mtspr   DBAT3U, r0
        mtspr   DBAT3L, r0
+       mtspr   DBAT4U, r0
+       mtspr   DBAT4L, r0
+       mtspr   DBAT5U, r0
+       mtspr   DBAT5L, r0
+       mtspr   DBAT6U, r0
+       mtspr   DBAT6L, r0
+       mtspr   DBAT7U, r0
+       mtspr   DBAT7L, r0
        mtspr   IBAT0U, r0
        mtspr   IBAT0L, r0
        mtspr   IBAT1U, r0
@@ -458,6 +433,14 @@ init_5xxx_core:
        mtspr   IBAT2L, r0
        mtspr   IBAT3U, r0
        mtspr   IBAT3L, r0
+       mtspr   IBAT4U, r0
+       mtspr   IBAT4L, r0
+       mtspr   IBAT5U, r0
+       mtspr   IBAT5L, r0
+       mtspr   IBAT6U, r0
+       mtspr   IBAT6L, r0
+       mtspr   IBAT7U, r0
+       mtspr   IBAT7L, r0
        SYNC
 
        /* invalidate all tlb's                                         */