Reworked the LEON3 startup 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 board_init_r.
Signed-off-by: Francois Retief fgretief@spaceteq.co.za ---
Changes in v2: - Add snoop detect function to restore lost feature
arch/sparc/cpu/leon3/cpu_init.c | 13 +- arch/sparc/cpu/leon3/start.S | 436 ++++++++++++++++++++++------------------ 2 files changed, 248 insertions(+), 201 deletions(-)
diff --git a/arch/sparc/cpu/leon3/cpu_init.c b/arch/sparc/cpu/leon3/cpu_init.c index 1144610..b0c7088 100644 --- a/arch/sparc/cpu/leon3/cpu_init.c +++ b/arch/sparc/cpu/leon3/cpu_init.c @@ -29,13 +29,23 @@ ambapp_dev_irqmp *irqmp = NULL; ambapp_dev_mctrl memctrl; ambapp_dev_gptimer *gptimer = NULL; unsigned int gptimer_irq = 0; -int leon3_snooping_avail = 0; +int leon3_snooping_avail = 0; /* used by arch/sparc/cpu/leon3/usb_uhci.c */
struct { gd_t gd_area; bd_t bd; } global_data;
+/* If CACHE snooping is available in hardware the result will be set + * to 0x800000, otherwise 0. + */ +static unsigned int snoop_detect(void) +{ + unsigned int result; + asm("lda [%%g0] 2, %0" : "=r"(result)); + return result & 0x00800000; +} + /* * Breath some life into the CPU... * @@ -92,6 +102,7 @@ void cpu_init_f(void) irqmp->cpu_force[0] = 0; /* no force IRQ on CPU 0 */
/* cache */ + leon3_snooping_avail = snoop_detect(); }
#ifndef CONFIG_SYS_GENERIC_BOARD diff --git a/arch/sparc/cpu/leon3/start.S b/arch/sparc/cpu/leon3/start.S index cf897f6..5cd791c 100644 --- a/arch/sparc/cpu/leon3/start.S +++ b/arch/sparc/cpu/leon3/start.S @@ -179,15 +179,6 @@ _trap_table: SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! f4-f7 SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! f8-fb SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! fc-ff -/* - * Version string - */ - - .data - .extern leon3_snooping_avail - .globl version_string -version_string: - .ascii U_BOOT_VERSION_STRING, "\0"
.section ".text" .align 4 @@ -232,151 +223,237 @@ clear_window: bge clear_window save
-wininit: +wiminit: set WIM_INIT, %g3 mov %g3, %wim
-stackp: +stack_init: set CONFIG_SYS_INIT_SP_OFFSET, %fp andn %fp, 0x0f, %fp sub %fp, 64, %sp
+tbr_init: + set CONFIG_SYS_TEXT_BASE, %g2 + wr %g0, %g2, %tbr + nop + nop + nop + +#ifdef CONFIG_TARGET_GR712RC_BOARD /* can't do this here, if we run from flash, we will be writing to flash! */ +clr_bss: + call 1f ! Use call to get hold of address for data table + nop + /* addresses of BSS segment */ + .word __bss_start ! 0x08 + .word __bss_end ! 0x0C +1: ld [%o7+0x08], %g2 ! __bss_start + ld [%o7+0x0C], %g3 ! __bss_stop + /* clearing 16byte a time ==> linker script need to align to 16 byte offset */ + clr %g1 /* std %g0 uses g0 and g1 */ +2: std %g0, [%g2] + std %g0, [%g2+8] + inc 16, %g2 ! ptr += 16 + cmp %g2, %g3 + bcs 2b ! while (ptr < end) + nop +#endif + cpu_init_unreloc: call cpu_init_f - nop + nop
-/* un relocated start address of monitor */ -#define TEXT_START _text - -/* un relocated end address of monitor */ -#define DATA_END __init_end - -reloc: - set TEXT_START,%g2 - set DATA_END,%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 - - clr %l0 - clr %l1 - clr %l2 - clr %l3 - clr %g2 +board_init_unreloc: + call board_init_f + clr %o0 ! boot_flags + +dead: + mov 1, %g1 ! For GRMON2 to exit normally + ta 0 ! if call returns.. (unlikely) + nop + +!-------------------------------------------------------------------------------
-/* register g4 contain address to start - * This means that BSS must be directly after data and code segments +/* + * void relocate_code (addr_sp, gd, addr_moni) * - * g3 is length of bss = (__bss_end-__bss_start) + * This "function" does not return, instead it continues in RAM + * after relocating the monitor code. * + * %i0 = Relocated stack pointer + * %i1 = Relocated global data pointer + * %i2 = Relocated text pointer */ - -clr_bss: -/* clear bss area (the relocated) */ - set __bss_start,%g2 - set __bss_end,%g3 - sub %g3,%g2,%g3 - add %g3,%g4,%g3 - 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 + .globl relocate_code + .type relocate_code, #function +relocate_code: + call 1f ! Use call to get hold of the actual address our data table + nop + /* final function to call */ + .word board_init_r ! 0x00 + /* start address of text segment */ + .word _text ! 0x04 + /* addresses of BSS segment */ + .word __bss_start ! 0x08 + .word __bss_end ! 0x0C + /* addresses of GOT segment */ + .word __got_start ! 0x10 + .word __got_end ! 0x14 +1: + add %o7, 0x08, %g5 ! data table address + + /* disable traps */ +! mov %psr, %l0 +! and %l0, 0xFFFFFFDF, %l0 +! mov %l0, %psr + + ! %o0 = Relocated stack pointer + ! %o1 = Relocated global data pointer + ! %o2 = Relocated text pointer + ! %o7 = Actual address of relocate_code + + ! %g5 = Data table address + ! %g6 = Relocation offset + +_reloc: + ld [%g5+0x04], %g2 ! _text + sub %o2, %g2, %g6 ! relocation offset + ld [%g5+0x0C], %g3 ! __bss_stop + mov %o2, %g4 ! relocation pointer + /* copy text & bss to relocated address */ +10: ldd [%g2], %l0 + ldd [%g2+8], %l2 + std %l0, [%g4] + std %l2, [%g4+8] + inc 16, %g2 + cmp %g2, %g3 + bcs 10b ! while (ptr < end) + inc 16, %g4 + + /* add offsets to GOT table */ +_fixup_got: + ld [%g5+0x10], %g4 ! __got_start + add %g4, %g6, %g4 + ld [%g5+0x14], %g3 ! __got_end + add %g3, %g6, %g3 +30: ld [%g4], %l0 +#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 + cmp %g4, %g3 + bcs 30b ! while (ptr < end) + nop + + ! %o0 = stack pointer (relocated) + ! %o1 = global data pointer (relocated) + ! %o2 = text pointer (relocated) + + ! %g5 = data table address + ! %g6 = relocation offset + + /* Trap table has been moved, tell CPU about new trap table address */ +_update_trap_table_address: + wr %g0, %o2, %tbr + nop + nop nop
-/* add offsets to GOT table */ -fixup_got: - set __got_start,%g4 - set __got_end,%g3 +_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 + + ld [%g5+0x00], %o3 ! board_init_r + add %o3, %g6, %o3 ! relocate address + call %o3 + nop + + mov 1, %g1 ! should not reach this point + ta 0 + nop + +!------------------------------------------------------------------------------- + /* - * new got offset = (old GOT-PTR (read with ld) - - * CONFIG_SYS_RELOC_MONITOR_BASE(from define) ) + - * Destination Address (from define) + * Window overflow trap handler */ - set CONFIG_SYS_RELOC_MONITOR_BASE,%g2 - set TEXT_START, %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 + .global _window_overflow +_window_overflow: + mov %wim, %l3 ! Calculate next WIM + sll %l3, (CONFIG_SYS_SPARC_NWINDOWS-1) , %l4 + srl %l3, 1, %l3 + or %l3, %l4, %l3 + + mov %g0, %wim ! Disable WIM traps + nop + nop nop
-prom_relocate: - set __prom_start, %g2 - set __prom_end, %g3 - set CONFIG_SYS_PROM_OFFSET, %g4 - -prom_relocate_loop: - ldd [%g2],%l0 - ldd [%g2+8],%l2 - std %l0,[%g4] - std %l2,[%g4+8] - inc 16,%g2 - subcc %g3,%g2,%g0 - bne prom_relocate_loop - inc 16,%g4 - -/* Trap table has been moved, lets tell CPU about - * the new trap table address - */ + save ! Get into window to be saved
- set CONFIG_SYS_RELOC_MONITOR_BASE, %g2 - wr %g0, %g2, %tbr + std %l0, [%sp + 0] ! Save window to the stack + std %l2, [%sp + 8] + std %l4, [%sp + 16] + std %l6, [%sp + 24] + std %i0, [%sp + 32] + std %i2, [%sp + 40] + std %i4, [%sp + 48] + std %i6, [%sp + 56] + + restore ! Go back to trap window + + mov %l3, %wim ! Set new WIM trap nop nop nop
-/* If CACHE snooping is available in hardware the - * variable leon3_snooping_avail will be set to - * 0x800000 else 0. + jmp %l1 ! Re-execute save + rett %l2 + +!------------------------------------------------------------------------------- + +/* + * Window underflow trap handler */ -snoop_detect: - sethi %hi(0x00800000), %o0 - lda [%g0] 2, %o1 - and %o0, %o1, %o0 - sethi %hi(leon3_snooping_avail+CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %o1 - st %o0, [%lo(leon3_snooping_avail+CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE)+%o1] - -/* call relocate*/ + .global _window_underflow +_window_underflow: + mov %wim, %l3 ! Calculate next WIM + srl %l3, (CONFIG_SYS_SPARC_NWINDOWS-1), %l4 + sll %l3, 1, %l3 + or %l3, %l4, %l3 + + mov %l3, %wim ! set new value of window + nop + nop nop -/* Call relocated init functions */ -jump: - set cpu_init_f2,%o1 - set CONFIG_SYS_RELOC_MONITOR_BASE,%o2 - add %o1,%o2,%o1 - sub %o1,%g1,%o1 - call %o1 - clr %o0
- set board_init_f,%o1 - set CONFIG_SYS_RELOC_MONITOR_BASE,%o2 - add %o1,%o2,%o1 - sub %o1,%g1,%o1 - call %o1 - clr %o0 + restore ! Two restores to get into the + restore ! window to restore
-dead: ta 0 ! if call returns... - nop + ldd [%sp + 0], %l0 ! Restore window from the stack + ldd [%sp + 8], %l2 + ldd [%sp + 16], %l4 + ldd [%sp + 24], %l6 + ldd [%sp + 32], %i0 + ldd [%sp + 40], %i2 + ldd [%sp + 48], %i4 + ldd [%sp + 56], %i6 + + save ! Get back to the trap window + save + + jmp %l1 ! Re-execute restore + rett %l2 + +!-------------------------------------------------------------------------------
/* Interrupt handler caller, * reg L7: interrupt number @@ -386,7 +463,25 @@ dead: ta 0 ! if call returns... * reg L3: wim */ _irq_entry: - SAVE_ALL + mov %o7, %l5 ! save original %o7 + call 1f + ld [%o7+0x08], %l4 ! trap_setup + .word trap_setup ! 0x08 +#ifdef CONFIG_SYS_GENERIC_BOARD + .word _irq_entry ! 0x0c + !SAVE_ALL +1: + ld [%o7+0x0C], %l6 ! _irq_entry + sub %o7, %l6, %l6 ! relocation offset (+4) + sub %l6, 4, %l6 ! relocation offset +#else + .word handler_irq ! 0x0c + !SAVE_ALL +1: set (CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %l6 +#endif + add %l4, %l6, %l4 ! relocated trap_setup + jmpl %l4, %l6 + mov %l5, %o7 ! restore orignal %o7
or %l0, PSR_PIL, %g2 wr %g2, 0x0, %psr @@ -394,10 +489,15 @@ _irq_entry: wr %g2, PSR_ET, %psr WRITE_PAUSE mov %l7, %o0 ! irq level - set handler_irq, %o1 +#ifdef CONFIG_SYS_GENERIC_BOARD + call handler_irq ! relative jump to handle_irq +#else + sub %l6, 0x18, %o2 ! calculate _irq_entry address + ld [%o2 + 0x0c], %o1 ! handler_irq set (CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %o2 add %o1, %o2, %o1 call %o1 +#endif add %sp, SF_REGS_SZ, %o1 ! pt_regs ptr or %l0, PSR_PIL, %g2 ! restore PIL after handler_irq wr %g2, PSR_ET, %psr ! keep ET up @@ -405,79 +505,9 @@ _irq_entry:
RESTORE_ALL
-!Window overflow trap handler. - .global _window_overflow - -_window_overflow: - - mov %wim, %l3 ! Calculate next WIM - mov %g1, %l7 - srl %l3, 1, %g1 - 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 - st %l0, [%sp + 0]; - st %l1, [%sp + 4]; - st %l2, [%sp + 8]; - st %l3, [%sp + 12]; - st %l4, [%sp + 16]; - st %l5, [%sp + 20]; - st %l6, [%sp + 24]; - st %l7, [%sp + 28]; - st %i0, [%sp + 32]; - st %i1, [%sp + 36]; - st %i2, [%sp + 40]; - st %i3, [%sp + 44]; - st %i4, [%sp + 48]; - st %i5, [%sp + 52]; - st %i6, [%sp + 56]; - st %i7, [%sp + 60]; - restore ! Go back to trap window. - mov %l7, %g1 - jmp %l1 ! Re-execute save. - rett %l2 - -/* Window underflow trap handler. */ - - .global _window_underflow - -_window_underflow: - - mov %wim, %l3 ! Calculate next WIM - sll %l3, 1, %l4 - srl %l3, (CONFIG_SYS_SPARC_NWINDOWS-1), %l5 - or %l5, %l4, %l5 - mov %l5, %wim - nop; nop; nop - restore ! Two restores to get into the - restore ! window to restore - ld [%sp + 0], %l0; ! Restore window from the stack - ld [%sp + 4], %l1; - ld [%sp + 8], %l2; - ld [%sp + 12], %l3; - ld [%sp + 16], %l4; - ld [%sp + 20], %l5; - ld [%sp + 24], %l6; - ld [%sp + 28], %l7; - ld [%sp + 32], %i0; - ld [%sp + 36], %i1; - ld [%sp + 40], %i2; - ld [%sp + 44], %i3; - ld [%sp + 48], %i4; - ld [%sp + 52], %i5; - ld [%sp + 56], %i6; - ld [%sp + 60], %i7; - save ! Get back to the trap window. - save - jmp %l1 ! Re-execute restore. - rett %l2 - - retl
_nmi_trap: nop @@ -597,6 +627,12 @@ ret_trap_entry: * It resets the system by jumping to _start */ _reset_reloc: - set start, %l0 +#if 1 /* DEBUG: When we run in GRMON, use the trap to exit normally. */ + mov 1, %g1 + ta 0 ! Cause program to exit normally in GRMON + nop +#else + set CONFIG_SYS_RELOC_MONITOR_BASE, %l0 call %l0 - nop + nop +#endif -- 1.9.3
________________________________ Disclaimer and confidentiality note – refer to our website for further details: www.spaceteq.co.za http://www.spaceteq.co.za/home/emaildisclaimer/