MIPS: fix iand optimize setup of CP0 registers

Clear cp0 status while preserving implementation specific bits.
Set bits BEV and ERL as the arch specification requires after
a reset or soft-reset exception.

Extend and fix initialization of watch registers. Check if additional
watch register sets are implemented and initialize them too.

Initialize cp0 count as early as possible to get the most
accurate boot timing.

Signed-off-by: Daniel Schwierzeck <daniel.schwierzeck@gmail.com>

diff --git a/arch/mips/cpu/start.S b/arch/mips/cpu/start.S
index 2397b6cc82380b52fc2acbd9b67b35dc85de682b..1ad1884a8d20b5efedf84528ac5f0225d873af90 100644 (file)
--- a/arch/mips/cpu/start.S
+++ b/arch/mips/cpu/start.S
@@ -34,24 +34,16 @@
 # define STATUS_SET    ST0_KX
 #endif
 
-       /*
-        * For the moment disable interrupts, mark the kernel mode and
-        * set ST0_KX so that the CPU does not spit fire when using
-        * 64-bit addresses.
-        */
-       .macro  setup_c0_status set clr
-       .set    push
-       mfc0    t0, CP0_STATUS
-       or      t0, ST0_CU0 | \set | 0x1f | \clr
-       xor     t0, 0x1f | \clr
-       mtc0    t0, CP0_STATUS
-       .set    noreorder
-       sll     zero, 3                         # ehb
-       .set    pop
-       .endm
-
        .set noreorder
 
+       .macro init_wr sel
+       MTC0    zero, CP0_WATCHLO,\sel
+       mtc0    t1, CP0_WATCHHI,\sel
+       mfc0    t0, CP0_WATCHHI,\sel
+       bgez    t0, wr_done
+        nop
+       .endm
+
        .macro uhi_mips_exception
        move    k0, t9          # preserve t9 in k0
        move    k1, a0          # preserve a0 in k1
@@ -63,7 +55,7 @@
 ENTRY(_start)
        /* U-Boot entry point */
        b       reset
-        nop
+        mtc0   zero, CP0_COUNT # clear cp0 count for most accurate boot timing
 
 #if defined(CONFIG_SYS_XWAY_EBU_BOOTCFG)
        /*
@@ -141,17 +133,51 @@ reset:
        b       3b
         nop
 
-       /* Clear watch registers */
-4:     MTC0    zero, CP0_WATCHLO
+       /* Init CP0 Status */
+4:     mfc0    t0, CP0_STATUS
+       and     t0, ST0_IMPL
+       or      t0, ST0_BEV | ST0_ERL | STATUS_SET
+       mtc0    t0, CP0_STATUS
+
+       /*
+        * Check whether CP0 Config1 is implemented. If not continue
+        * with legacy Watch register initialization.
+        */
+       mfc0    t0, CP0_CONFIG
+       bgez    t0, wr_legacy
+        nop
+
+       /*
+        * Check WR bit in CP0 Config1 to determine if Watch registers
+        * are implemented.
+        */
+       mfc0    t0, CP0_CONFIG, 1
+       andi    t0, (1 << 3)
+       beqz    t0, wr_done
+        nop
+
+       /* Clear Watch Status bits and disable watch exceptions */
+       li      t1, 0x7         # Clear I, R and W conditions
+       init_wr 0
+       init_wr 1
+       init_wr 2
+       init_wr 3
+       init_wr 4
+       init_wr 5
+       init_wr 6
+       init_wr 7
+       b       wr_done
+        nop
+
+wr_legacy:
+       MTC0    zero, CP0_WATCHLO
        mtc0    zero, CP0_WATCHHI
 
-       /* WP(Watch Pending), SW0/1 should be cleared */
+wr_done:
+       /* Clear WP, IV and SW interrupts */
        mtc0    zero, CP0_CAUSE
 
-       setup_c0_status STATUS_SET 0
-
-       /* Init Timer */
-       mtc0    zero, CP0_COUNT
+       /* Clear timer interrupt (CP0_COUNT cleared on branch to 'reset') */
        mtc0    zero, CP0_COMPARE
 
 #ifndef CONFIG_SKIP_LOWLEVEL_INIT

diff --git a/arch/mips/include/asm/mipsregs.h b/arch/mips/include/asm/mipsregs.h
index 9ab506361e7897fb76ad0c8cdd37ebf3e58afc7a..7a9d222aadecfc1ed32e91e12b46aefa49634511 100644 (file)
--- a/arch/mips/include/asm/mipsregs.h
+++ b/arch/mips/include/asm/mipsregs.h
@@ -299,6 +299,7 @@
 #define         STATUSF_IP14           (_ULCAST_(1) <<  6)
 #define         STATUSB_IP15           7
 #define         STATUSF_IP15           (_ULCAST_(1) <<  7)
+#define ST0_IMPL               (_ULCAST_(3) <<  16)
 #define ST0_CH                 0x00040000
 #define ST0_NMI                        0x00080000
 #define ST0_SR                 0x00100000