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 2397b6c..1ad1884 100644
--- 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 @@
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 9ab5063..7a9d222 100644
--- 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