From 65d297af7c6a08ec65bf005fa9bd5cdb955efe39 Mon Sep 17 00:00:00 2001 From: Daniel Schwierzeck Date: Mon, 8 Feb 2016 00:37:59 +0100 Subject: [PATCH] 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 --- arch/mips/cpu/start.S | 74 +++++++++++++++++++++----------- arch/mips/include/asm/mipsregs.h | 1 + 2 files changed, 51 insertions(+), 24 deletions(-) diff --git a/arch/mips/cpu/start.S b/arch/mips/cpu/start.S index 2397b6cc82..1ad1884a8d 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 @@ 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 9ab506361e..7a9d222aad 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 -- 2.39.5