[U-Boot] [PATCH 16/20] iMX28: Fix ARM vector handling

[Marek Vasut <marek.vasut@gmail.com>]

This patch introduces proper ARM vector handling for i.MX28 CPU. This issue
wasn't addressed because the interrupts weren't enabled on any ARMv5 core,
therefore the issue wasn't noticed earlier.

In previous implementation, the vectoring code used by i.MX28 CPU when an
exception happened was that of the SPL. With this change, the branch target when
an exception happens can be reconfigured by U-Boot.

Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
Cc: Stefano Babic <sbabic@denx.de>
Cc: Wolfgang Denk <wd@denx.de>
Cc: Detlev Zundel <dzu@denx.de>

---
 arch/arm/cpu/arm926ejs/mx28/mx28.c |   22 +++
 board/denx/m28evk/start.S          |  264 +++++++-----------------------------
 include/configs/m28evk.h           |    1 +
 3 files changed, 74 insertions(+), 213 deletions(-)

diff --git a/arch/arm/cpu/arm926ejs/mx28/mx28.c b/arch/arm/cpu/arm926ejs/mx28/mx28.c
index e990f3c..088c019 100644
--- a/arch/arm/cpu/arm926ejs/mx28/mx28.c
+++ b/arch/arm/cpu/arm926ejs/mx28/mx28.c
@@ -35,6 +35,8 @@
 #include <asm/arch/imx-regs.h>
 #include <asm/arch/sys_proto.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 /* 1 second delay should be plenty of time for block reset. */
 #define	RESET_MAX_TIMEOUT	1000000
 
@@ -116,11 +118,31 @@ int mx28_reset_block(struct mx28_register *reg)
 	return 0;
 }
 
+void mx28_fixup_vt(uint32_t start_addr)
+{
+	uint32_t *vt = (uint32_t *)0x20;
+	int i;
+
+	for (i = 0; i < 8; i++)
+		vt[i] = start_addr + (4 * i);
+}
+
+#ifdef	CONFIG_ARCH_MISC_INIT
+int arch_misc_init(void)
+{
+	mx28_fixup_vt(gd->relocaddr);
+	return 0;
+}
+#endif
+
 #ifdef	CONFIG_ARCH_CPU_INIT
 int arch_cpu_init(void)
 {
 	struct mx28_clkctrl_regs *clkctrl_regs =
 		(struct mx28_clkctrl_regs *)MXS_CLKCTRL_BASE;
+	extern uint32_t _start;
+
+	mx28_fixup_vt((uint32_t)&_start);
 
 	/*
 	 * Enable NAND clock
diff --git a/board/denx/m28evk/start.S b/board/denx/m28evk/start.S
index cf67599..94696d6 100644
--- a/board/denx/m28evk/start.S
+++ b/board/denx/m28evk/start.S
@@ -58,54 +58,58 @@
 .globl _start
 _start:
 	b	reset
-#ifdef CONFIG_SPL_BUILD
-/* No exception handlers in preloader */
-	ldr	pc, _hang
-	ldr	pc, _hang
-	ldr	pc, _hang
-	ldr	pc, _hang
-	b	reset
-	ldr	pc, _hang
-	ldr	pc, _hang
+	b	undefined_instruction
+	b	software_interrupt
+	b	prefetch_abort
+	b	data_abort
+	b	not_used
+	b	irq
+	b	fiq
 
-_hang:
-	.word	do_hang
-/* pad to 64 byte boundary */
-	.word	0x12345678
-	.word	0x12345678
-	.word	0x12345678
-	.word	0x12345678
-	.word	0x12345678
-	.word	0x12345678
-	.word	0x12345678
-#else
-	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
+/*
+ * Vector table, located at address 0x20.
+ * This table allows the code running AFTER SPL, the U-Boot, to install it's
+ * interrupt handlers here. The problem is that the U-Boot is loaded into RAM,
+ * including it's interrupt vectoring table and the table at 0x0 is still the
+ * 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
 
-_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
+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
 
-#endif	/* CONFIG_SPL_BUILD */
 	.balignl 16,0xdeadbeef
 
-
 /*
  *************************************************************************
  *
@@ -162,7 +166,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.
@@ -220,177 +224,11 @@ cpu_init_crit:
 	mcr	p15, 0, r0, c1, c0, 0
 
 	mov	pc, lr		/* back to my caller */
-#endif /* CONFIG_SKIP_LOWLEVEL_INIT */
-
-#ifndef CONFIG_SPL_BUILD
-/*
- *************************************************************************
- *
- * Interrupt handling
- *
- *************************************************************************
- */
-
-@
-@ IRQ stack frame.
-@
-#define S_FRAME_SIZE	72
-
-#define S_OLD_R0	68
-#define S_PSR		64
-#define S_PC		60
-#define S_LR		56
-#define S_SP		52
-
-#define S_IP		48
-#define S_FP		44
-#define S_R10		40
-#define S_R9		36
-#define S_R8		32
-#define S_R7		28
-#define S_R6		24
-#define S_R5		20
-#define S_R4		16
-#define S_R3		12
-#define S_R2		8
-#define S_R1		4
-#define S_R0		0
-
-#define MODE_SVC 0x13
-#define I_BIT	 0x80
-
-/*
- * use bad_save_user_regs for abort/prefetch/undef/swi ...
- * use irq_save_user_regs / irq_restore_user_regs for IRQ/FIQ handling
- */
-
-	.macro	bad_save_user_regs
-	@ carve out a frame on current user stack
-	sub	sp, sp, #S_FRAME_SIZE
-	stmia	sp, {r0 - r12}	@ Save user registers (now in svc mode) r0-r12
-	ldr	r2, IRQ_STACK_START_IN
-	@ get values for "aborted" pc and cpsr (into parm regs)
-	ldmia	r2, {r2 - r3}
-	add	r0, sp, #S_FRAME_SIZE		@ grab pointer to old stack
-	add	r5, sp, #S_SP
-	mov	r1, lr
-	stmia	r5, {r0 - r3}	@ save sp_SVC, lr_SVC, pc, cpsr
-	mov	r0, sp		@ save current stack into r0 (param register)
-	.endm
-
-	.macro	irq_save_user_regs
-	sub	sp, sp, #S_FRAME_SIZE
-	stmia	sp, {r0 - r12}			@ Calling r0-r12
-	@ !!!! R8 NEEDS to be saved !!!! a reserved stack spot would be good.
-	add	r8, sp, #S_PC
-	stmdb	r8, {sp, lr}^		@ Calling SP, LR
-	str	lr, [r8, #0]		@ Save calling PC
-	mrs	r6, spsr
-	str	r6, [r8, #4]		@ Save CPSR
-	str	r0, [r8, #8]		@ Save OLD_R0
-	mov	r0, sp
-	.endm
-
-	.macro	irq_restore_user_regs
-	ldmia	sp, {r0 - lr}^			@ Calling r0 - lr
-	mov	r0, r0
-	ldr	lr, [sp, #S_PC]			@ Get PC
-	add	sp, sp, #S_FRAME_SIZE
-	subs	pc, lr, #4		@ return & move spsr_svc into cpsr
-	.endm
-
-	.macro get_bad_stack
-	ldr	r13, IRQ_STACK_START_IN		@ setup our mode stack
-
-	str	lr, [r13]	@ save caller lr in position 0 of saved stack
-	mrs	lr, spsr	@ get the spsr
-	str	lr, [r13, #4]	@ save spsr in position 1 of saved stack
-	mov	r13, #MODE_SVC	@ prepare SVC-Mode
-	@ msr	spsr_c, r13
-	msr	spsr, r13	@ switch modes, make sure moves will execute
-	mov	lr, pc		@ capture return pc
-	movs	pc, lr		@ jump to next instruction & switch modes.
-	.endm
-
-	.macro get_irq_stack			@ setup IRQ stack
-	ldr	sp, IRQ_STACK_START
-	.endm
-
-	.macro get_fiq_stack			@ setup FIQ stack
-	ldr	sp, FIQ_STACK_START
-	.endm
-#endif	/* CONFIG_SPL_BUILD */
 
-/*
- * exception handlers
- */
-#ifdef CONFIG_SPL_BUILD
 	.align	5
-do_hang:
+#endif /* CONFIG_SKIP_LOWLEVEL_INIT */
+
+_hang:
 	ldr	sp, _TEXT_BASE			/* switch to abort stack */
 1:
 	bl	1b				/* hang and never return */
-#else	/* !CONFIG_SPL_BUILD */
-	.align  5
-undefined_instruction:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_undefined_instruction
-
-	.align	5
-software_interrupt:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_software_interrupt
-
-	.align	5
-prefetch_abort:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_prefetch_abort
-
-	.align	5
-data_abort:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_data_abort
-
-	.align	5
-not_used:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_not_used
-
-#ifdef CONFIG_USE_IRQ
-
-	.align	5
-irq:
-	get_irq_stack
-	irq_save_user_regs
-	bl	do_irq
-	irq_restore_user_regs
-
-	.align	5
-fiq:
-	get_fiq_stack
-	/* someone ought to write a more effiction fiq_save_user_regs */
-	irq_save_user_regs
-	bl	do_fiq
-	irq_restore_user_regs
-
-#else
-
-	.align	5
-irq:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_irq
-
-	.align	5
-fiq:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_fiq
-
-#endif
-#endif	/* CONFIG_SPL_BUILD */
diff --git a/include/configs/m28evk.h b/include/configs/m28evk.h
index 59e3e05..381b01e 100644
--- a/include/configs/m28evk.h
+++ b/include/configs/m28evk.h
@@ -41,6 +41,7 @@
 #define	CONFIG_SYS_DCACHE_OFF
 #define	CONFIG_BOARD_EARLY_INIT_F
 #define	CONFIG_ARCH_CPU_INIT
+#define	CONFIG_ARCH_MISC_INIT
 
 /*
  * SPL
-- 
1.7.7.1