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>

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

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