sparc: leon2: Updates for generic board initialization

Reworked the LEON2 start.S code to call board_init_f function at startup.
Also implemented the relocate_code function in assembly to relocate the
monitor and setup the stack pointer before calling relocated board_init_r.

Add the CONFIG_SYS_GENERIC_BOARD variable to all the LEON2 boards.

Signed-off-by: Francois Retief <fgretief@spaceteq.co.za>

3 files changed, 111 insertions, 77 deletions

diff --git a/arch/sparc/cpu/leon2/cpu_init.c b/arch/sparc/cpu/leon2/cpu_init.c
index 581f057..5630b09 100644
--- a/arch/sparc/cpu/leon2/cpu_init.c
+++ b/arch/sparc/cpu/leon2/cpu_init.c
@@ -56,11 +56,6 @@ void cpu_init_f(void)
 #endif
 }
 
-void cpu_init_f2(void)
-{
-
-}
-
 int arch_cpu_init(void)
 {
 	gd->cpu_clk = CONFIG_SYS_CLK_FREQ;
diff --git a/arch/sparc/cpu/leon2/start.S b/arch/sparc/cpu/leon2/start.S
index fa1534f..7362ae1 100644
--- a/arch/sparc/cpu/leon2/start.S
+++ b/arch/sparc/cpu/leon2/start.S
@@ -310,30 +310,62 @@ leon2_init_stackp:
 	andn	%fp, 0x0f, %fp
 	sub	%fp, 64, %sp
 
+leon2_init_tbr:
+	set	CONFIG_SYS_TEXT_BASE, %g2
+	wr	%g0, %g2, %tbr
+	nop
+	nop
+	nop
+
 cpu_init_unreloc:
 	call	cpu_init_f
 	 nop
 
+board_init_unreloc:
+	call	board_init_f
+	 clr	%o0			! boot_flags
+
+dead_unreloc:
+	ba	dead_unreloc		! infinte loop
+	 nop
+
+!-------------------------------------------------------------------------------
+
+/* void relocate_code (addr_sp, gd, addr_moni)
+ *
+ * This "function" does not return, instead it continues in RAM after
+ * relocating the monitor code.
+ *
+ * %o0 = Relocated stack pointer
+ * %o1 = Relocated global data pointer
+ * %o2 = Relocated text pointer
+ */
+	.globl	relocate_code
+	.type	relocate_code, #function
+	.align	4
+relocate_code:
+	SPARC_PIC_THUNK_CALL(l7)
+
 /* un relocated start address of monitor */
 #define TEXT_START _text
 
 /* un relocated end address of monitor */
 #define DATA_END __init_end
 
-	SPARC_PIC_THUNK_CALL(l7)
 reloc:
 	SPARC_LOAD_ADDRESS(TEXT_START, l7, g2)
 	SPARC_LOAD_ADDRESS(DATA_END, l7, g3)
-	set	CONFIG_SYS_RELOC_MONITOR_BASE,%g4
-reloc_loop:
-	ldd	[%g2],%l0
-	ldd	[%g2+8],%l2
-	std	%l0,[%g4]
-	std	%l2,[%g4+8]
-	inc	16,%g2
-	subcc	%g3,%g2,%g0
-	bne	reloc_loop
-	 inc	16,%g4
+	mov	%o2, %g4		! relocation address
+	sub	%g4, %g2, %g6		! relocation offset
+	/* copy .text & .data to relocated address */
+10:	ldd	[%g2], %l0
+	ldd	[%g2+8], %l2
+	std	%l0, [%g4]
+	std	%l2, [%g4+8]
+	inc	16, %g2			! src += 16
+	cmp	%g2, %g3
+	bcs	10b			! while (src < end)
+	 inc	16, %g4			! dst += 16
 
 	clr	%l0
 	clr	%l1
@@ -348,49 +380,42 @@ reloc_loop:
  *
  */
 
+	/* clear bss area (the relocated) */
 clr_bss:
-/* clear bss area (the relocated) */
 	SPARC_LOAD_ADDRESS(__bss_start, l7, g2)
 	SPARC_LOAD_ADDRESS(__bss_end, l7, g3)
-	sub	%g3,%g2,%g3
+	sub	%g3,%g2,%g3		! length of .bss area
 	add	%g3,%g4,%g3
+	/* clearing 16byte a time ==> linker script need to align to 16 byte offset */
 	clr	%g1	/* std %g0 uses g0 and g1 */
-/* clearing 16byte a time ==> linker script need to align to 16 byte offset */
-clr_bss_16:
-	std	%g0,[%g4]
-	std	%g0,[%g4+8]
-	inc	16,%g4
-	cmp	%g3,%g4
-	bne	clr_bss_16
+20:
+	std	%g0, [%g4]
+	std	%g0, [%g4+8]
+	inc	16, %g4			! ptr += 16
+	cmp	%g4, %g3
+	bcs	20b			! while (ptr < end)
 	 nop
 
-/* add offsets to GOT table */
+	/* add offsets to GOT table */
 fixup_got:
 	SPARC_LOAD_ADDRESS(__got_start, l7, g4)
+	add	%g4, %g6, %g4
 	SPARC_LOAD_ADDRESS(__got_end, l7, g3)
-/*
- * new got offset = (old GOT-PTR (read with ld) -
- *   CONFIG_SYS_RELOC_MONITOR_BASE(from define) ) +
- *   Destination Address (from define)
- */
-	set	CONFIG_SYS_RELOC_MONITOR_BASE,%g2
-	SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
-	add	%g4,%g2,%g4
-	sub	%g4,%g1,%g4
-	add	%g3,%g2,%g3
-	sub	%g3,%g1,%g3
-	sub	%g2,%g1,%g2	! prepare register with (new base address) -
-				!  (old base address)
-got_loop:
-	ld	[%g4],%l0	! load old GOT-PTR
-	add	%l0,%g2,%l0	! increase with (new base address) -
-				!  (old base)
-	st	%l0,[%g4]
-	inc	4,%g4
-	cmp	%g3,%g4
-	bne	got_loop
+	add	%g3, %g6, %g3
+30:	ld	[%g4], %l0		! load old GOT-PTR
+#ifdef CONFIG_RELOC_GOT_SKIP_NULL
+	cmp	%l0, 0
+	be	32f
+#endif
+	add	%l0, %g6, %l0		! relocate GOT pointer
+	st	%l0, [%g4]
+32:	inc	4, %g4			! ptr += 4
+	cmp	%g4, %g3
+	bcs	30b			! while (ptr < end)
 	 nop
 
+#if 0 /* FIXME: Relocated PROM address should be calculated! */
+
 prom_relocate:
 	SPARC_LOAD_ADDRESS(__prom_start, l7, g2)
 	SPARC_LOAD_ADDRESS(__prom_end, l7, g3)
@@ -406,33 +431,42 @@ prom_relocate_loop:
 	bne	prom_relocate_loop
 	 inc	16,%g4
 
+#endif
+
+! %o0 = stack pointer (relocated)
+! %o1 = global data pointer (relocated)
+! %o2 = text pointer (relocated)
+
+! %g6 = relocation offset
+! %l7 = _GLOBAL_OFFSET_TABLE_
+
 /* Trap table has been moved, lets tell CPU about
  * the new trap table address
  */
-
-	set	CONFIG_SYS_RELOC_MONITOR_BASE, %g2
-	wr	%g0, %g2, %tbr
-
-/*	call	relocate*/
+update_trap_table_address:
+	wr	%g0, %o2, %tbr
+	nop
+	nop
 	nop
-/* Call relocated init functions */
-jump:
-	SPARC_LOAD_ADDRESS(cpu_init_f2, l7, o1)
-	set	CONFIG_SYS_RELOC_MONITOR_BASE,%o2
-	add	%o1,%o2,%o1
-	sub	%o1,%g1,%o1
-	call	%o1
-	 clr	%o0
 
-	SPARC_LOAD_ADDRESS(board_init_f, l7, o1)
-	set	CONFIG_SYS_RELOC_MONITOR_BASE,%o2
-	add	%o1,%o2,%o1
-	sub	%o1,%g1,%o1
-	call	%o1
-	 clr	%o0
+update_stack_pointers:
+	mov	%o0, %fp
+	andn	%fp, 0x0f, %fp	! align to 16 bytes
+	add	%fp, -64, %fp	! make space for a window push
+	mov	%fp, %sp	! setup stack pointer
+
+jump_board_init_r:
+	mov	%o1, %o0	! relocated global data pointer
+	mov	%o2, %o1	! relocated text pointer
+	SPARC_LOAD_ADDRESS(board_init_r, l7, o3)
+	add	%o3, %g6, %o3	! add relocation offset
+	call	%o3
+	 nop
 
 dead:	ta 0				! if call returns...
-	nop
+	 nop
+
+!------------------------------------------------------------------------------
 
 /* Interrupt handler caller,
  * reg L7: interrupt number
@@ -461,7 +495,11 @@ _irq_entry:
 
 	RESTORE_ALL
 
-!Window overflow trap handler.
+!------------------------------------------------------------------------------
+
+/*
+ * Window overflow trap handler.
+ */
 	.global _window_overflow
 
 _window_overflow:
@@ -469,14 +507,12 @@ _window_overflow:
 	mov	%wim, %l3		! Calculate next WIM
 	mov	%g1, %l7
 	srl	%l3, 1, %g1
-	sll	%l3, (CONFIG_SYS_SPARC_NWINDOWS-1) , %l4
+	sll	%l3, (CONFIG_SYS_SPARC_NWINDOWS-1), %l4
 	or	%l4, %g1, %g1
 
 	save				! Get into window to be saved.
 	mov	%g1, %wim
-	nop;
-	nop;
-	nop
+	nop; nop; nop
 	st	%l0, [%sp + 0];
 	st	%l1, [%sp + 4];
 	st	%l2, [%sp + 8];
@@ -498,8 +534,9 @@ _window_overflow:
 	jmp	%l1			! Re-execute save.
 	rett	%l2
 
-/* Window underflow trap handler.  */
-
+/*
+ * Window underflow trap handler.
+ */
 	.global  _window_underflow
 
 _window_underflow:
@@ -533,7 +570,7 @@ _window_underflow:
 	jmp	%l1			! Re-execute restore.
 	rett	%l2
 
-	retl
+!------------------------------------------------------------------------------
 
 _nmi_trap:
 	nop
diff --git a/include/configs/grsim_leon2.h b/include/configs/grsim_leon2.h
index 83022e8..2f7a562 100644
--- a/include/configs/grsim_leon2.h
+++ b/include/configs/grsim_leon2.h
@@ -3,8 +3,8 @@
  * (C) Copyright 2003-2005
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
- * (C) Copyright 2007
- * Daniel Hellstrom, Gaisler Research, daniel@gaisler.com.
+ * (C) Copyright 2007, 2015
+ * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -12,6 +12,8 @@
 #ifndef __CONFIG_H__
 #define __CONFIG_H__
 
+#define CONFIG_SYS_GENERIC_BOARD
+
 /*
  * High Level Configuration Options
  * (easy to change)