[PATCH v2] arm: mxs: Clear CPSR V bit to activate low vectors

[Marek Vasut]

The MXS starts with CPSR V bit set, which makes the CPU jump to high vectors
in case of an exception. Those high vectors are located at 0xffff0000, which
is where the BootROM exception table is located as well. U-Boot should handle
exceptions on its own using its own exception handling code, which is located
at 0x0, i.e. at low vectors. Clear the CPSR V bit, so that the CPU would jump
to low vectors on exception instead, and therefore run the U-Boot exception
handling code.

Signed-off-by: Marek Vasut <marex at denx.de>
---
Cc: "NXP i.MX U-Boot Team" <uboot-imx at nxp.com>
Cc: Fabio Estevam <festevam at gmail.com>
Cc: Lukasz Majewski <lukma at denx.de>
Cc: Stefano Babic <sbabic at denx.de>
---
V2: Mark both get_cr()/set_cr() callsites as __attribute__((target("arm")))
    and noinline to force generation of ARM version of those functions,
    as this core cannot run mrc/mcr in Thumb mode. This is truly hideous
    workaround right here.
---
 arch/arm/cpu/arm926ejs/mxs/mxs.c      | 4 ++++
 arch/arm/cpu/arm926ejs/mxs/spl_boot.c | 8 +++++++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/arch/arm/cpu/arm926ejs/mxs/mxs.c b/arch/arm/cpu/arm926ejs/mxs/mxs.c
index 6d6166cb839..4f3cb63c56d 100644
--- a/arch/arm/cpu/arm926ejs/mxs/mxs.c
+++ b/arch/arm/cpu/arm926ejs/mxs/mxs.c
@@ -71,6 +71,7 @@ void reset_cpu(void)
  * actually 0x20, this the associated <destination address>. Loading the PC
  * register with an address performs a jump to that address.
  */
+noinline __attribute__((target("arm")))
 void mx28_fixup_vt(uint32_t start_addr)
 {
 	/* ldr pc, [pc, #0x18] */
@@ -85,6 +86,9 @@ void mx28_fixup_vt(uint32_t start_addr)
 		/* cppcheck-suppress nullPointer */
 		vt[i + 8] = start_addr + (4 * i);
 	}
+
+	/* Make sure ARM core points to low vectors */
+	set_cr(get_cr() & ~CR_V);
 }
 
 #ifdef	CONFIG_ARCH_MISC_INIT
diff --git a/arch/arm/cpu/arm926ejs/mxs/spl_boot.c b/arch/arm/cpu/arm926ejs/mxs/spl_boot.c
index 5e7bdb78be1..249f8de8fbe 100644
--- a/arch/arm/cpu/arm926ejs/mxs/spl_boot.c
+++ b/arch/arm/cpu/arm926ejs/mxs/spl_boot.c
@@ -17,6 +17,7 @@
 #include <asm/arch/sys_proto.h>
 #include <asm/gpio.h>
 #include <asm/sections.h>
+#include <asm/system.h>
 #include <linux/compiler.h>
 
 #include "mxs_init.h"
@@ -93,7 +94,9 @@ static uint8_t mxs_get_bootmode_index(void)
 	return i;
 }
 
-static void mxs_spl_fixup_vectors(void)
+static noinline
+__attribute__((target("arm")))
+void mxs_spl_fixup_vectors(void)
 {
 	/*
 	 * Copy our vector table to 0x0, since due to HAB, we cannot
@@ -104,6 +107,9 @@ static void mxs_spl_fixup_vectors(void)
 
 	/* cppcheck-suppress nullPointer */
 	memcpy(0x0, _start, 0x60);
+
+	/* Make sure ARM core points to low vectors */
+	set_cr(get_cr() & ~CR_V);
 }
 
 static void mxs_spl_console_init(void)
-- 
2.42.0