From 9e99698ff9601d291ccb6eaf75242ba7f957a302 Mon Sep 17 00:00:00 2001
From: =?utf8?q?Lothar=20Wa=C3=9Fmann?= <LW@KARO-electronics.de>
Date: Thu, 10 Jul 2014 11:09:20 +0200
Subject: [PATCH] mxs: change setup of exception vt

---
 arch/arm/cpu/arm926ejs/mxs/mxs.c   | 11 ++--
 arch/arm/cpu/arm926ejs/mxs/start.S | 89 ++++++++++++++----------------
 2 files changed, 45 insertions(+), 55 deletions(-)

diff --git a/arch/arm/cpu/arm926ejs/mxs/mxs.c b/arch/arm/cpu/arm926ejs/mxs/mxs.c
index bf29f0ae08..6f09f627ae 100644
--- a/arch/arm/cpu/arm926ejs/mxs/mxs.c
+++ b/arch/arm/cpu/arm926ejs/mxs/mxs.c
@@ -109,15 +109,12 @@ void enable_caches(void)
 void mx28_fixup_vt(uint32_t start_addr)
 {
 	/* ldr pc, [pc, #0x18] */
-	const uint32_t ldr_pc = 0xe59ff018;
 	/* Jumptable location is 0x0 */
-	uint32_t *vt = (uint32_t *)0x0;
-	int i;
+	uint32_t *vt = (uint32_t *)0x20;
+	uint32_t cr = get_cr();
 
-	for (i = 0; i < 8; i++) {
-		vt[i] = ldr_pc;
-		vt[i + 8] = start_addr + (4 * i);
-	}
+	memcpy(vt, (void *)start_addr + 0x20, 32);
+	set_cr(cr & ~CR_V);
 }
 
 #ifdef	CONFIG_ARCH_MISC_INIT
diff --git a/arch/arm/cpu/arm926ejs/mxs/start.S b/arch/arm/cpu/arm926ejs/mxs/start.S
index 94b2b3fd30..6a32038d34 100644
--- a/arch/arm/cpu/arm926ejs/mxs/start.S
+++ b/arch/arm/cpu/arm926ejs/mxs/start.S
@@ -36,14 +36,30 @@
 .globl _start
 _start:
 	b	reset
-	b	undefined_instruction
-	b	software_interrupt
-	b	prefetch_abort
-	b	data_abort
-	b	not_used
-	b	irq
-	b	fiq
-
+	ldr	pc, _undefined_instruction
+	ldr	pc, _software_interrupt
+	ldr	pc, _prefetch_abort
+	ldr	pc, _data_abort
+	ldr	pc, _not_used
+	ldr	pc, _irq
+	ldr	pc, _fiq
+
+#ifdef CONFIG_SPL_BUILD
+_undefined_instruction:
+	.word	_undefined_instruction
+_software_interrupt:
+	.word	_software_interrupt
+_prefetch_abort:
+	.word	_prefetch_abort
+_data_abort:
+	.word	_data_abort
+_not_used:
+	.word	_not_used
+_irq:
+	.word	_irq
+_fiq:
+	.word	_fiq
+#else
 /*
  * Vector table, located at address 0x20.
  * This table allows the code running AFTER SPL, the U-Boot, to install it's
@@ -52,41 +68,22 @@ _start:
  * SPLs. So if interrupt happens in U-Boot, the SPLs interrupt vectoring table
  * is still used.
  */
-_vt_reset:
-	.word	_reset
-_vt_undefined_instruction:
-	.word	_hang
-_vt_software_interrupt:
-	.word	_hang
-_vt_prefetch_abort:
-	.word	_hang
-_vt_data_abort:
-	.word	_hang
-_vt_not_used:
-	.word	_reset
-_vt_irq:
-	.word	_hang
-_vt_fiq:
-	.word	_hang
-
-reset:
-	ldr	pc, _vt_reset
-undefined_instruction:
-	ldr	pc, _vt_undefined_instruction
-software_interrupt:
-	ldr	pc, _vt_software_interrupt
-prefetch_abort:
-	ldr	pc, _vt_prefetch_abort
-data_abort:
-	ldr	pc, _vt_data_abort
-not_used:
-	ldr	pc, _vt_not_used
-irq:
-	ldr	pc, _vt_irq
-fiq:
-	ldr	pc, _vt_fiq
-
-	.balignl 16,0xdeadbeef
+_undefined_instruction:
+	.word	undefined_instruction
+_software_interrupt:
+	.word	software_interrupt
+_prefetch_abort:
+	.word	prefetch_abort
+_data_abort:
+	.word	data_abort
+_not_used:
+	.word	not_used
+_irq:
+	.word	irq
+_fiq:
+	.word	fiq
+#endif /* CONFIG_SPL_BUILD */
+	.balignl 16, 0xdeadbeef
 
 /*
  *************************************************************************
@@ -148,7 +145,7 @@ IRQ_STACK_START_IN:
  * the actual reset code
  */
 
-_reset:
+reset:
 	/*
 	 * Store all registers on old stack pointer, this will allow us later to
 	 * return to the BootROM and let the BootROM load U-Boot into RAM.
@@ -187,7 +184,3 @@ _reset:
 	pop	{r0-r12,r14}
 	bx	lr
 
-_hang:
-	ldr	sp, _TEXT_BASE			/* switch to abort stack */
-1:
-	bl	1b				/* hang and never return */
-- 
2.39.2