[people/ms/u-boot.git] / arch / sparc / cpu / leon3 / start.S

/* This is where the SPARC/LEON3 starts
 *
 * Copyright (C) 2007, 2015
 * Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <asm-offsets.h>
#include <config.h>
#include <asm/asmmacro.h>
#include <asm/winmacro.h>
#include <asm/psr.h>
#include <asm/stack.h>
#include <asm/leon.h>
#include <ambapp.h>

/* Default Plug&Play I/O area */
#ifndef CONFIG_AMBAPP_IOAREA
#define CONFIG_AMBAPP_IOAREA AMBA_DEFAULT_IOAREA
#endif

/* Default number of SPARC register windows */
#ifndef CONFIG_SYS_SPARC_NWINDOWS
#define CONFIG_SYS_SPARC_NWINDOWS 8
#endif

/* Entry for traps which jump to a programmer-specified trap handler.  */
#define TRAPR(H)  \
	wr	%g0, 0xfe0, %psr; \
	mov	%g0, %tbr; \
	ba	(H); \
	mov	%g0, %wim;

#define TRAP(H) \
	mov	%psr, %l0; \
	ba	(H); \
	nop; nop;

#define TRAPI(ilevel) \
	mov	ilevel, %l7; \
	mov	%psr, %l0; \
	b	_irq_entry; \
	mov	%wim, %l3

/* Unexcpected trap will halt the processor by forcing it to error state */
#undef BAD_TRAP
#define BAD_TRAP ta 0; nop; nop; nop;

/* Software trap. Treat as BAD_TRAP for the time being... */
#define SOFT_TRAP TRAP(_hwerr)

#define PSR_INIT   0x1FC0	/* Disable traps, set s and ps */
#define WIM_INIT   2

/* All traps low-level code here must end with this macro. */
#define RESTORE_ALL b ret_trap_entry; clr %l6;

#define WRITE_PAUSE nop;nop;nop

WINDOWSIZE = (16 * 4)
ARGPUSHSIZE = (6 * 4)
ARGPUSH = (WINDOWSIZE + 4)
MINFRAME = (WINDOWSIZE + ARGPUSHSIZE + 4)

/* Number of register windows */
#ifndef CONFIG_SYS_SPARC_NWINDOWS
#error Must define number of SPARC register windows, default is 8
#endif

/* Macros to load address into a register. Uses GOT table for PIC */
#ifdef __PIC__

#define SPARC_PIC_THUNK_CALL(reg) \
	sethi	%pc22(_GLOBAL_OFFSET_TABLE_-4), %##reg; \
	call	__sparc_get_pc_thunk.reg; \
	 add	%##reg, %pc10(_GLOBAL_OFFSET_TABLE_+4), %##reg;

#define SPARC_LOAD_ADDRESS(sym, got, reg) \
	sethi	%gdop_hix22(sym), %##reg; \
	xor	%##reg, %gdop_lox10(sym), %##reg; \
	ld	[%##got + %##reg], %##reg, %gdop(sym);

#else

#define SPARC_PIC_THUNK_CALL(reg)
#define SPARC_LOAD_ADDRESS(sym, got, tmp) \
	set	sym, %##reg;

#endif

#define STACK_ALIGN	8
#define SA(X)	(((X)+(STACK_ALIGN-1)) & ~(STACK_ALIGN-1))

	.section ".start", "ax"
	.globl	_start, start, _trap_table
	.globl  _irq_entry, nmi_trap
	.globl  _reset_reloc

/* at address 0
 * Hardware traps
 */
start:
_start:
_trap_table:
	TRAPR(_hardreset);		! 00 reset trap
	BAD_TRAP;			! 01 instruction_access_exception
	BAD_TRAP;			! 02 illegal_instruction
	BAD_TRAP;			! 03 priveleged_instruction
	BAD_TRAP;			! 04 fp_disabled
	TRAP(_window_overflow);		! 05 window_overflow
	TRAP(_window_underflow);	! 06 window_underflow
	BAD_TRAP;			! 07 Memory Address Not Aligned
	BAD_TRAP;			! 08 Floating Point Exception
	BAD_TRAP;			! 09 Data Miss Exception
	BAD_TRAP;			! 0a Tagged Instruction Ovrflw
	BAD_TRAP;			! 0b Watchpoint Detected
	BAD_TRAP;			! 0c
	BAD_TRAP;			! 0d
	BAD_TRAP;			! 0e
	BAD_TRAP;			! 0f
	BAD_TRAP;			! 10
	TRAPI(1);			! 11 IRQ level 1
	TRAPI(2);			! 12 IRQ level 2
	TRAPI(3);			! 13 IRQ level 3
	TRAPI(4);			! 14 IRQ level 4
	TRAPI(5);			! 15 IRQ level 5
	TRAPI(6);			! 16 IRQ level 6
	TRAPI(7);			! 17 IRQ level 7
	TRAPI(8);			! 18 IRQ level 8
	TRAPI(9);			! 19 IRQ level 9
	TRAPI(10);			! 1a IRQ level 10
	TRAPI(11);			! 1b IRQ level 11
	TRAPI(12);			! 1c IRQ level 12
	TRAPI(13);			! 1d IRQ level 13
	TRAPI(14);			! 1e IRQ level 14
	TRAP(_nmi_trap);		! 1f IRQ level 15 /
					! NMI (non maskable interrupt)
	BAD_TRAP;			! 20 r_register_access_error
	BAD_TRAP;			! 21 instruction access error
	BAD_TRAP;			! 22
	BAD_TRAP;			! 23
	BAD_TRAP;			! 24 co-processor disabled
	BAD_TRAP;			! 25 uniplemented FLUSH
	BAD_TRAP;			! 26
	BAD_TRAP;			! 27
	BAD_TRAP;			! 28 co-processor exception
	BAD_TRAP;			! 29 data access error
	BAD_TRAP;			! 2a division by zero
	BAD_TRAP;			! 2b data store error
	BAD_TRAP;			! 2c data access MMU miss
	BAD_TRAP;			! 2d
	BAD_TRAP;			! 2e
	BAD_TRAP;			! 2f
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 30-33
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 34-37
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 38-3b
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 3c-3f
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 40-43
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 44-47
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 48-4b
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 4c-4f
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 50-53
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 54-57
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 58-5b
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 5c-5f

	/* implementaion dependent */
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 60-63
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 64-67
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 68-6b
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 6c-6f
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 70-73
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 74-77
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 78-7b
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 7c-7f

	/* Software traps, not handled */
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 80-83
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 84-87
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 88-8b
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 8c-8f
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 90-93
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 94-97
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 98-9b
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 9c-9f
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! a0-a3
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! a4-a7
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! a8-ab
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! ac-af
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! b0-b3
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! b4-b7
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! b8-bb
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! bc-bf
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! c0-c3
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! c4-c7
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! c8-cb
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! cc-cf
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! d0-d3
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! d4-d7
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! d8-db
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! dc-df
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! e0-e3
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! e4-e7
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! e8-eb
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! ec-ef
	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! f0-f3
	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

	.section	".text"
	.extern	_nomem_amba_init, _nomem_memory_ctrl_init
	.align 4

_hardreset:
1000:
	flush

	/* Enable I/D-Cache and Snooping */
	set	0x0081000f, %g2
	sta	%g2, [%g0] 2

	mov	%g0, %y
	clr	%g1
	clr	%g2
	clr	%g3
	clr	%g4
	clr	%g5
	clr	%g6
	clr	%g7

	mov	%asr17, %g3
	and	%g3, 0x1f, %g3
clear_window:
	mov	%g0, %l0
	mov	%g0, %l1
	mov	%g0, %l2
	mov	%g0, %l3
	mov	%g0, %l4
	mov	%g0, %l5
	mov	%g0, %l6
	mov	%g0, %l7
	mov	%g0, %o0
	mov	%g0, %o1
	mov	%g0, %o2
	mov	%g0, %o3
	mov	%g0, %o4
	mov	%g0, %o5
	mov	%g0, %o6
	mov	%g0, %o7
	subcc	%g3, 1, %g3
	bge	clear_window
	save

wiminit:
	set	WIM_INIT, %g3
	mov	%g3, %wim

stackp:
	set	CONFIG_SYS_INIT_SP_OFFSET, %fp
	andn	%fp, 0x0f, %fp
	sub	%fp, 64, %sp

/* Obtain the address of _GLOBAL_OFFSET_TABLE_ */
	SPARC_PIC_THUNK_CALL(l7)

/* Scan AMBA Bus for AMBA buses using PnP information. All found
 * AMBA buses I/O area will be located in i0-i5 upon return.
 * The i0-i5 registers are later used by _nomem_amba_init2
 */
ambainit:
	call	_nomem_amba_init
	 sethi	%hi(CONFIG_AMBAPP_IOAREA), %o0

/* Scan AMBA Buses for memory controllers, then initialize the
 * memory controllers. Note that before setting up the memory controller
 * the stack can not be used.
 */
memory_ctrl_init:
	SPARC_LOAD_ADDRESS(grlib_mctrl_handlers, l7, o0)

	call	_nomem_memory_ctrl_init
	 nop

/* The return valu indicate how many memory controllers where found and
 * initialized, if no memory controller was initialized, we can not continue
 * because from here on we expect memory to be working.
 */
	cmp	%o0, 0
memory_ctrl_init_failed:
	beq	memory_ctrl_init_failed
	 nop

/*** From now on the stack can be used. ***/

cpu_init_unreloc:
	call	cpu_init_f
	 nop

/* un relocated start address of monitor */
#define TEXT_START _text

/* un relocated end address of monitor */
#define DATA_END __init_end

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

	clr	%l0
	clr	%l1
	clr	%l2
	clr	%l3
	clr	%g2

/* register g4 contain address to start
 * This means that BSS must be directly after data and code segments
 *
 * g3 is length of bss = (__bss_end-__bss_start)
 *
 */

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
	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
	 nop

/* add offsets to GOT table */
fixup_got:
	SPARC_LOAD_ADDRESS(__got_start, l7, 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
	 nop

prom_relocate:
	SPARC_LOAD_ADDRESS(__prom_start, l7, g2)
	SPARC_LOAD_ADDRESS(__prom_end, l7, 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
 */

	set	CONFIG_SYS_RELOC_MONITOR_BASE, %g2
	wr	%g0, %g2, %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
	SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
	add	%o1,%o2,%o1
	sub	%o1,%g1,%o1
	call	%o1
	 clr	%o0

dead:	ta 0				! if call returns...
	nop

/* Interrupt handler caller,
 * reg L7: interrupt number
 * reg L0: psr after interrupt
 * reg L1: PC
 * reg L2: next PC
 * reg L3: wim
 */
_irq_entry:
	SAVE_ALL

	or	%l0, PSR_PIL, %g2
	wr	%g2, 0x0, %psr
	WRITE_PAUSE
	wr	%g2, PSR_ET, %psr
	WRITE_PAUSE
	mov	%l7, %o0		! irq level
	set	handler_irq, %o1
	set	(CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %o2
	add	%o1, %o2, %o1
	call	%o1
	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
	WRITE_PAUSE

	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
	jmp %l1
	rett %l2

_hwerr:
	ta 0
	nop
	nop
	b _hwerr			! loop infinite
	nop

/* Registers to not touch at all. */
#define t_psr      l0 /* Set by caller */
#define t_pc       l1 /* Set by caller */
#define t_npc      l2 /* Set by caller */
#define t_wim      l3 /* Set by caller */
#define t_twinmask l4 /* Set at beginning of this entry routine. */
#define t_kstack   l5 /* Set right before pt_regs frame is built */
#define t_retpc    l6 /* If you change this, change winmacro.h header file */
#define t_systable l7 /* Never touch this, could be the syscall table ptr. */
#define curptr     g6 /* Set after pt_regs frame is built */

trap_setup:
/* build a pt_regs trap frame. */
	sub	%fp, (SF_REGS_SZ + PT_REGS_SZ), %t_kstack
	PT_STORE_ALL(t_kstack, t_psr, t_pc, t_npc, g2)

	/* See if we are in the trap window. */
	mov	1, %t_twinmask
	sll	%t_twinmask, %t_psr, %t_twinmask ! t_twinmask = (1 << psr)
	andcc	%t_twinmask, %t_wim, %g0
	beq	1f		! in trap window, clean up
	nop

	/*-------------------------------------------------
	 * Spill , adjust %wim and go.
	 */
	srl	%t_wim, 0x1, %g2		! begin computation of new %wim

	set	(CONFIG_SYS_SPARC_NWINDOWS-1), %g3	!NWINDOWS-1

	sll	%t_wim, %g3, %t_wim	! NWINDOWS-1
	or	%t_wim, %g2, %g2
	and	%g2, 0xff, %g2

	save	%g0, %g0, %g0           ! get in window to be saved

	/* Set new %wim value */
	wr	%g2, 0x0, %wim

	/* Save the kernel window onto the corresponding stack. */
	RW_STORE(sp)

	restore	%g0, %g0, %g0
	/*-------------------------------------------------*/

1:
	/* Trap from kernel with a window available.
	 * Just do it...
	 */
	jmpl	%t_retpc + 0x8, %g0	! return to caller
	 mov	%t_kstack, %sp		! jump onto new stack

#define twin_tmp1 l4
#define glob_tmp  g4
#define curptr    g6
ret_trap_entry:
	wr	%t_psr, 0x0, %psr       ! enable nesting again, clear ET

	/* Will the rett land us in the invalid window? */
	mov	2, %g1
	sll	%g1, %t_psr, %g1

	set	CONFIG_SYS_SPARC_NWINDOWS, %g2	!NWINDOWS

	srl	%g1, %g2, %g2
	or	%g1, %g2, %g1
	rd	%wim, %g2
	andcc	%g2, %g1, %g0
	be	1f		! Nope, just return from the trap
	 sll	%g2, 0x1, %g1

	/* We have to grab a window before returning. */
	set	(CONFIG_SYS_SPARC_NWINDOWS-1), %g3	!NWINDOWS-1

	srl	%g2, %g3,  %g2
	or	%g1, %g2, %g1
	and	%g1, 0xff, %g1

	wr	%g1, 0x0, %wim

	/* Grrr, make sure we load from the right %sp... */
	PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)

	restore	%g0, %g0, %g0
	RW_LOAD(sp)
	b	2f
	save	%g0, %g0, %g0

	/* Reload the entire frame in case this is from a
	 * kernel system call or whatever...
	 */
1:
	PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)
2:
	wr	%t_psr, 0x0, %psr
	nop;
	nop;
	nop

	jmp	%t_pc
	rett	%t_npc

/* This is called from relocated C-code.
 * It resets the system by jumping to _start
 */
_reset_reloc:
	set	start, %l0
	call	%l0
	nop
Commit	Line	Data
e8a8bab5	1	/* This is where the SPARC/LEON3 starts
be7357a6 FR	2	*
	3	* Copyright (C) 2007, 2015
	4	* Daniel Hellstrom, Cobham Gaisler, daniel@gaisler.com
e8a8bab5	5	*
e8a8bab5 TR	6	* SPDX-License-Identifier: GPL-2.0+
	7	*/
	8
	9	#include <asm-offsets.h>
76512e0d	10	#include <config.h>
e8a8bab5 TR	11	#include <asm/asmmacro.h>
	12	#include <asm/winmacro.h>
	13	#include <asm/psr.h>
	14	#include <asm/stack.h>
	15	#include <asm/leon.h>
cff009ed DH	16	#include <ambapp.h>
	17
	18	/* Default Plug&Play I/O area */
	19	#ifndef CONFIG_AMBAPP_IOAREA
	20	#define CONFIG_AMBAPP_IOAREA AMBA_DEFAULT_IOAREA
	21	#endif
e8a8bab5	22
52789143 FR	23	/* Default number of SPARC register windows */
	24	#ifndef CONFIG_SYS_SPARC_NWINDOWS
	25	#define CONFIG_SYS_SPARC_NWINDOWS 8
	26	#endif
	27
e8a8bab5 TR	28	/* Entry for traps which jump to a programmer-specified trap handler. */
	29	#define TRAPR(H) \
	30	wr %g0, 0xfe0, %psr; \
	31	mov %g0, %tbr; \
	32	ba (H); \
	33	mov %g0, %wim;
	34
	35	#define TRAP(H) \
	36	mov %psr, %l0; \
	37	ba (H); \
	38	nop; nop;
	39
	40	#define TRAPI(ilevel) \
	41	mov ilevel, %l7; \
	42	mov %psr, %l0; \
	43	b _irq_entry; \
	44	mov %wim, %l3
	45
	46	/* Unexcpected trap will halt the processor by forcing it to error state */
1e9a164e DH	47	#undef BAD_TRAP
	48	#define BAD_TRAP ta 0; nop; nop; nop;
	49
	50	/* Software trap. Treat as BAD_TRAP for the time being... */
	51	#define SOFT_TRAP TRAP(_hwerr)
	52
	53	#define PSR_INIT 0x1FC0 /* Disable traps, set s and ps */
	54	#define WIM_INIT 2
	55
	56	/* All traps low-level code here must end with this macro. */
	57	#define RESTORE_ALL b ret_trap_entry; clr %l6;
	58
	59	#define WRITE_PAUSE nop;nop;nop
	60
	61	WINDOWSIZE = (16 * 4)
	62	ARGPUSHSIZE = (6 * 4)
	63	ARGPUSH = (WINDOWSIZE + 4)
	64	MINFRAME = (WINDOWSIZE + ARGPUSHSIZE + 4)
	65
	66	/* Number of register windows */
6d0f6bcf	67	#ifndef CONFIG_SYS_SPARC_NWINDOWS
1e9a164e DH	68	#error Must define number of SPARC register windows, default is 8
	69	#endif
	70
0070109f FR	71	/* Macros to load address into a register. Uses GOT table for PIC */
	72	#ifdef __PIC__
	73
	74	#define SPARC_PIC_THUNK_CALL(reg) \
	75	sethi %pc22(_GLOBAL_OFFSET_TABLE_-4), %##reg; \
	76	call __sparc_get_pc_thunk.reg; \
	77	add %##reg, %pc10(_GLOBAL_OFFSET_TABLE_+4), %##reg;
	78
	79	#define SPARC_LOAD_ADDRESS(sym, got, reg) \
	80	sethi %gdop_hix22(sym), %##reg; \
	81	xor %##reg, %gdop_lox10(sym), %##reg; \
	82	ld [%##got + %##reg], %##reg, %gdop(sym);
	83
	84	#else
	85
	86	#define SPARC_PIC_THUNK_CALL(reg)
	87	#define SPARC_LOAD_ADDRESS(sym, got, tmp) \
	88	set sym, %##reg;
	89
	90	#endif
	91
1e9a164e DH	92	#define STACK_ALIGN 8
	93	#define SA(X) (((X)+(STACK_ALIGN-1)) & ~(STACK_ALIGN-1))
	94
	95	.section ".start", "ax"
53677ef1	96	.globl _start, start, _trap_table
1e9a164e DH	97	.globl _irq_entry, nmi_trap
	98	.globl _reset_reloc
	99
	100	/* at address 0
	101	* Hardware traps
	102	*/
	103	start:
	104	_start:
	105	_trap_table:
	106	TRAPR(_hardreset); ! 00 reset trap
	107	BAD_TRAP; ! 01 instruction_access_exception
	108	BAD_TRAP; ! 02 illegal_instruction
	109	BAD_TRAP; ! 03 priveleged_instruction
	110	BAD_TRAP; ! 04 fp_disabled
	111	TRAP(_window_overflow); ! 05 window_overflow
	112	TRAP(_window_underflow); ! 06 window_underflow
	113	BAD_TRAP; ! 07 Memory Address Not Aligned
	114	BAD_TRAP; ! 08 Floating Point Exception
	115	BAD_TRAP; ! 09 Data Miss Exception
	116	BAD_TRAP; ! 0a Tagged Instruction Ovrflw
	117	BAD_TRAP; ! 0b Watchpoint Detected
	118	BAD_TRAP; ! 0c
	119	BAD_TRAP; ! 0d
	120	BAD_TRAP; ! 0e
	121	BAD_TRAP; ! 0f
	122	BAD_TRAP; ! 10
	123	TRAPI(1); ! 11 IRQ level 1
	124	TRAPI(2); ! 12 IRQ level 2
	125	TRAPI(3); ! 13 IRQ level 3
	126	TRAPI(4); ! 14 IRQ level 4
	127	TRAPI(5); ! 15 IRQ level 5
	128	TRAPI(6); ! 16 IRQ level 6
	129	TRAPI(7); ! 17 IRQ level 7
	130	TRAPI(8); ! 18 IRQ level 8
	131	TRAPI(9); ! 19 IRQ level 9
	132	TRAPI(10); ! 1a IRQ level 10
	133	TRAPI(11); ! 1b IRQ level 11
	134	TRAPI(12); ! 1c IRQ level 12
	135	TRAPI(13); ! 1d IRQ level 13
	136	TRAPI(14); ! 1e IRQ level 14
	137	TRAP(_nmi_trap); ! 1f IRQ level 15 /
53677ef1	138	! NMI (non maskable interrupt)
1e9a164e DH	139	BAD_TRAP; ! 20 r_register_access_error
	140	BAD_TRAP; ! 21 instruction access error
	141	BAD_TRAP; ! 22
	142	BAD_TRAP; ! 23
	143	BAD_TRAP; ! 24 co-processor disabled
	144	BAD_TRAP; ! 25 uniplemented FLUSH
	145	BAD_TRAP; ! 26
	146	BAD_TRAP; ! 27
	147	BAD_TRAP; ! 28 co-processor exception
	148	BAD_TRAP; ! 29 data access error
	149	BAD_TRAP; ! 2a division by zero
	150	BAD_TRAP; ! 2b data store error
	151	BAD_TRAP; ! 2c data access MMU miss
	152	BAD_TRAP; ! 2d
	153	BAD_TRAP; ! 2e
	154	BAD_TRAP; ! 2f
	155	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 30-33
	156	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 34-37
	157	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 38-3b
	158	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 3c-3f
	159	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 40-43
	160	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 44-47
	161	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 48-4b
	162	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 4c-4f
	163	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 50-53
	164	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 54-57
	165	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 58-5b
	166	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 5c-5f
	167
	168	/* implementaion dependent */
	169	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 60-63
	170	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 64-67
	171	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 68-6b
	172	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 6c-6f
	173	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 70-73
	174	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 74-77
	175	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 78-7b
	176	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 7c-7f
	177
	178	/* Software traps, not handled */
	179	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 80-83
	180	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 84-87
	181	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 88-8b
	182	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 8c-8f
	183	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 90-93
	184	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 94-97
	185	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 98-9b
	186	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! 9c-9f
	187	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! a0-a3
	188	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! a4-a7
	189	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! a8-ab
	190	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! ac-af
	191	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! b0-b3
	192	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! b4-b7
	193	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! b8-bb
	194	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! bc-bf
	195	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! c0-c3
	196	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! c4-c7
	197	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! c8-cb
	198	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! cc-cf
	199	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! d0-d3
	200	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! d4-d7
	201	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! d8-db
	202	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! dc-df
203	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! e0-e3
204	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! e4-e7
205	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! e8-eb
206	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! ec-ef
207	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! f0-f3
208	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! f4-f7
209	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! f8-fb
210	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; ! fc-ff
1e9a164e	211
1e9a164e	212	.section ".text"
cff009ed	213	.extern _nomem_amba_init, _nomem_memory_ctrl_init
1e9a164e DH	214	.align 4
	215
	216	_hardreset:
	217	1000:
	218	flush
	219
	220	/* Enable I/D-Cache and Snooping */
	221	set 0x0081000f, %g2
	222	sta %g2, [%g0] 2
	223
	224	mov %g0, %y
	225	clr %g1
	226	clr %g2
	227	clr %g3
	228	clr %g4
	229	clr %g5
	230	clr %g6
	231	clr %g7
	232
	233	mov %asr17, %g3
	234	and %g3, 0x1f, %g3
	235	clear_window:
	236	mov %g0, %l0
	237	mov %g0, %l1
	238	mov %g0, %l2
	239	mov %g0, %l3
	240	mov %g0, %l4
	241	mov %g0, %l5
	242	mov %g0, %l6
	243	mov %g0, %l7
	244	mov %g0, %o0
	245	mov %g0, %o1
	246	mov %g0, %o2
	247	mov %g0, %o3
	248	mov %g0, %o4
	249	mov %g0, %o5
	250	mov %g0, %o6
	251	mov %g0, %o7
	252	subcc %g3, 1, %g3
	253	bge clear_window
	254	save
	255
0070109f	256	wiminit:
1e9a164e DH	257	set WIM_INIT, %g3
	258	mov %g3, %wim
	259
	260	stackp:
6d0f6bcf	261	set CONFIG_SYS_INIT_SP_OFFSET, %fp
1e9a164e DH	262	andn %fp, 0x0f, %fp
	263	sub %fp, 64, %sp
	264
cff009ed DH	265	/* Obtain the address of _GLOBAL_OFFSET_TABLE_ */
	266	SPARC_PIC_THUNK_CALL(l7)
	267
	268	/* Scan AMBA Bus for AMBA buses using PnP information. All found
	269	* AMBA buses I/O area will be located in i0-i5 upon return.
	270	* The i0-i5 registers are later used by _nomem_amba_init2
	271	*/
	272	ambainit:
	273	call _nomem_amba_init
	274	sethi %hi(CONFIG_AMBAPP_IOAREA), %o0
	275
	276	/* Scan AMBA Buses for memory controllers, then initialize the
	277	* memory controllers. Note that before setting up the memory controller
	278	* the stack can not be used.
	279	*/
	280	memory_ctrl_init:
	281	SPARC_LOAD_ADDRESS(grlib_mctrl_handlers, l7, o0)
	282
	283	call _nomem_memory_ctrl_init
	284	nop
	285
	286	/* The return valu indicate how many memory controllers where found and
	287	* initialized, if no memory controller was initialized, we can not continue
	288	* because from here on we expect memory to be working.
	289	*/
	290	cmp %o0, 0
	291	memory_ctrl_init_failed:
	292	beq memory_ctrl_init_failed
	293	nop
	294
	295	/* From now on the stack can be used. */
	296
1e9a164e DH	297	cpu_init_unreloc:
1e9a164e DH	298	call cpu_init_f
0070109f	299	nop
1e9a164e DH	300
	301	/* un relocated start address of monitor */
	302	#define TEXT_START _text
	303
	304	/* un relocated end address of monitor */
	305	#define DATA_END __init_end
	306
	307	reloc:
0070109f FR	308	SPARC_LOAD_ADDRESS(TEXT_START, l7, g2)
0070109f FR	309	SPARC_LOAD_ADDRESS(DATA_END, l7, g3)
6d0f6bcf	310	set CONFIG_SYS_RELOC_MONITOR_BASE,%g4
1e9a164e DH	311	reloc_loop:
	312	ldd [%g2],%l0
	313	ldd [%g2+8],%l2
	314	std %l0,[%g4]
	315	std %l2,[%g4+8]
	316	inc 16,%g2
	317	subcc %g3,%g2,%g0
	318	bne reloc_loop
0070109f	319	inc 16,%g4
1e9a164e DH	320
	321	clr %l0
	322	clr %l1
	323	clr %l2
	324	clr %l3
	325	clr %g2
	326
	327	/* register g4 contain address to start
	328	* This means that BSS must be directly after data and code segments
	329	*
	330	* g3 is length of bss = (__bss_end-__bss_start)
	331	*
	332	*/
	333
	334	clr_bss:
	335	/* clear bss area (the relocated) */
0070109f FR	336	SPARC_LOAD_ADDRESS(__bss_start, l7, g2)
0070109f FR	337	SPARC_LOAD_ADDRESS(__bss_end, l7, g3)
1e9a164e DH	338	sub %g3,%g2,%g3
	339	add %g3,%g4,%g3
	340	clr %g1 /* std %g0 uses g0 and g1 */
	341	/* clearing 16byte a time ==> linker script need to align to 16 byte offset */
	342	clr_bss_16:
	343	std %g0,[%g4]
	344	std %g0,[%g4+8]
	345	inc 16,%g4
	346	cmp %g3,%g4
	347	bne clr_bss_16
0070109f	348	nop
1e9a164e DH	349
	350	/* add offsets to GOT table */
	351	fixup_got:
0070109f FR	352	SPARC_LOAD_ADDRESS(__got_start, l7, g4)
0070109f FR	353	SPARC_LOAD_ADDRESS(__got_end, l7, g3)
1e9a164e DH	354	/*
1e9a164e DH	355	* new got offset = (old GOT-PTR (read with ld) -
6d0f6bcf	356	* CONFIG_SYS_RELOC_MONITOR_BASE(from define) ) +
1e9a164e DH	357	* Destination Address (from define)
1e9a164e DH	358	*/
6d0f6bcf	359	set CONFIG_SYS_RELOC_MONITOR_BASE,%g2
0070109f	360	SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
1e9a164e DH	361	add %g4,%g2,%g4
	362	sub %g4,%g1,%g4
	363	add %g3,%g2,%g3
	364	sub %g3,%g1,%g3
	365	sub %g2,%g1,%g2 ! prepare register with (new base address) -
	366	! (old base address)
	367	got_loop:
	368	ld [%g4],%l0 ! load old GOT-PTR
	369	add %l0,%g2,%l0 ! increase with (new base address) -
	370	! (old base)
	371	st %l0,[%g4]
	372	inc 4,%g4
	373	cmp %g3,%g4
	374	bne got_loop
0070109f	375	nop
1e9a164e DH	376
1e9a164e DH	377	prom_relocate:
0070109f FR	378	SPARC_LOAD_ADDRESS(__prom_start, l7, g2)
0070109f FR	379	SPARC_LOAD_ADDRESS(__prom_end, l7, g3)
6d0f6bcf	380	set CONFIG_SYS_PROM_OFFSET, %g4
1e9a164e DH	381
	382	prom_relocate_loop:
	383	ldd [%g2],%l0
	384	ldd [%g2+8],%l2
	385	std %l0,[%g4]
	386	std %l2,[%g4+8]
	387	inc 16,%g2
	388	subcc %g3,%g2,%g0
	389	bne prom_relocate_loop
0070109f	390	inc 16,%g4
1e9a164e DH	391
	392	/* Trap table has been moved, lets tell CPU about
	393	* the new trap table address
	394	*/
	395
6d0f6bcf	396	set CONFIG_SYS_RELOC_MONITOR_BASE, %g2
1e9a164e DH	397	wr %g0, %g2, %tbr
	398	nop
	399	nop
	400	nop
1aeed8d7	401
1e9a164e DH	402	/* Call relocated init functions */
1e9a164e DH	403	jump:
0070109f	404	SPARC_LOAD_ADDRESS(cpu_init_f2, l7, o1)
6d0f6bcf	405	set CONFIG_SYS_RELOC_MONITOR_BASE,%o2
1e9a164e DH	406	add %o1,%o2,%o1
	407	sub %o1,%g1,%o1
	408	call %o1
0070109f	409	clr %o0
1e9a164e	410
0070109f	411	SPARC_LOAD_ADDRESS(board_init_f, l7, o1)
6d0f6bcf	412	set CONFIG_SYS_RELOC_MONITOR_BASE,%o2
cff009ed	413	SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
1e9a164e DH	414	add %o1,%o2,%o1
	415	sub %o1,%g1,%o1
	416	call %o1
0070109f	417	clr %o0
1e9a164e DH	418
	419	dead: ta 0 ! if call returns...
	420	nop
	421
	422	/* Interrupt handler caller,
	423	* reg L7: interrupt number
	424	* reg L0: psr after interrupt
	425	* reg L1: PC
	426	* reg L2: next PC
	427	* reg L3: wim
	428	*/
	429	_irq_entry:
	430	SAVE_ALL
	431
	432	or %l0, PSR_PIL, %g2
	433	wr %g2, 0x0, %psr
	434	WRITE_PAUSE
	435	wr %g2, PSR_ET, %psr
	436	WRITE_PAUSE
	437	mov %l7, %o0 ! irq level
	438	set handler_irq, %o1
14d0a02a	439	set (CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %o2
1e9a164e DH	440	add %o1, %o2, %o1
	441	call %o1
	442	add %sp, SF_REGS_SZ, %o1 ! pt_regs ptr
	443	or %l0, PSR_PIL, %g2 ! restore PIL after handler_irq
	444	wr %g2, PSR_ET, %psr ! keep ET up
	445	WRITE_PAUSE
	446
	447	RESTORE_ALL
	448
	449	!Window overflow trap handler.
	450	.global _window_overflow
	451
	452	_window_overflow:
	453
	454	mov %wim, %l3 ! Calculate next WIM
	455	mov %g1, %l7
	456	srl %l3, 1, %g1
6d0f6bcf	457	sll %l3, (CONFIG_SYS_SPARC_NWINDOWS-1) , %l4
1e9a164e DH	458	or %l4, %g1, %g1
	459
	460	save ! Get into window to be saved.
	461	mov %g1, %wim
	462	nop;
	463	nop;
	464	nop
	465	st %l0, [%sp + 0];
	466	st %l1, [%sp + 4];
	467	st %l2, [%sp + 8];
	468	st %l3, [%sp + 12];
	469	st %l4, [%sp + 16];
	470	st %l5, [%sp + 20];
	471	st %l6, [%sp + 24];
	472	st %l7, [%sp + 28];
	473	st %i0, [%sp + 32];
	474	st %i1, [%sp + 36];
	475	st %i2, [%sp + 40];
	476	st %i3, [%sp + 44];
	477	st %i4, [%sp + 48];
	478	st %i5, [%sp + 52];
	479	st %i6, [%sp + 56];
	480	st %i7, [%sp + 60];
	481	restore ! Go back to trap window.
	482	mov %l7, %g1
	483	jmp %l1 ! Re-execute save.
	484	rett %l2
	485
	486	/* Window underflow trap handler. */
	487
	488	.global _window_underflow
	489
	490	_window_underflow:
	491
	492	mov %wim, %l3 ! Calculate next WIM
	493	sll %l3, 1, %l4
6d0f6bcf	494	srl %l3, (CONFIG_SYS_SPARC_NWINDOWS-1), %l5
1e9a164e DH	495	or %l5, %l4, %l5
	496	mov %l5, %wim
	497	nop; nop; nop
	498	restore ! Two restores to get into the
	499	restore ! window to restore
53677ef1	500	ld [%sp + 0], %l0; ! Restore window from the stack
1e9a164e DH	501	ld [%sp + 4], %l1;
	502	ld [%sp + 8], %l2;
	503	ld [%sp + 12], %l3;
	504	ld [%sp + 16], %l4;
	505	ld [%sp + 20], %l5;
	506	ld [%sp + 24], %l6;
	507	ld [%sp + 28], %l7;
	508	ld [%sp + 32], %i0;
	509	ld [%sp + 36], %i1;
	510	ld [%sp + 40], %i2;
	511	ld [%sp + 44], %i3;
	512	ld [%sp + 48], %i4;
	513	ld [%sp + 52], %i5;
	514	ld [%sp + 56], %i6;
	515	ld [%sp + 60], %i7;
	516	save ! Get back to the trap window.
	517	save
	518	jmp %l1 ! Re-execute restore.
	519	rett %l2
	520
	521	retl
	522
	523	_nmi_trap:
	524	nop
	525	jmp %l1
	526	rett %l2
	527
	528	_hwerr:
	529	ta 0
	530	nop
	531	nop
53677ef1	532	b _hwerr ! loop infinite
1e9a164e DH	533	nop
	534
	535	/* Registers to not touch at all. */
	536	#define t_psr l0 /* Set by caller */
	537	#define t_pc l1 /* Set by caller */
	538	#define t_npc l2 /* Set by caller */
	539	#define t_wim l3 /* Set by caller */
	540	#define t_twinmask l4 /* Set at beginning of this entry routine. */
	541	#define t_kstack l5 /* Set right before pt_regs frame is built */
	542	#define t_retpc l6 /* If you change this, change winmacro.h header file */
	543	#define t_systable l7 /* Never touch this, could be the syscall table ptr. */
	544	#define curptr g6 /* Set after pt_regs frame is built */
	545
	546	trap_setup:
	547	/* build a pt_regs trap frame. */
	548	sub %fp, (SF_REGS_SZ + PT_REGS_SZ), %t_kstack
	549	PT_STORE_ALL(t_kstack, t_psr, t_pc, t_npc, g2)
	550
	551	/* See if we are in the trap window. */
	552	mov 1, %t_twinmask
	553	sll %t_twinmask, %t_psr, %t_twinmask ! t_twinmask = (1 << psr)
	554	andcc %t_twinmask, %t_wim, %g0
	555	beq 1f ! in trap window, clean up
	556	nop
	557
	558	/*-------------------------------------------------
	559	* Spill , adjust %wim and go.
	560	*/
	561	srl %t_wim, 0x1, %g2 ! begin computation of new %wim
	562
6d0f6bcf	563	set (CONFIG_SYS_SPARC_NWINDOWS-1), %g3 !NWINDOWS-1
1e9a164e DH	564
	565	sll %t_wim, %g3, %t_wim ! NWINDOWS-1
	566	or %t_wim, %g2, %g2
	567	and %g2, 0xff, %g2
	568
	569	save %g0, %g0, %g0 ! get in window to be saved
	570
	571	/* Set new %wim value */
	572	wr %g2, 0x0, %wim
	573
	574	/* Save the kernel window onto the corresponding stack. */
	575	RW_STORE(sp)
	576
	577	restore %g0, %g0, %g0
	578	/-------------------------------------------------/
	579
	580	1:
	581	/* Trap from kernel with a window available.
	582	* Just do it...
	583	*/
	584	jmpl %t_retpc + 0x8, %g0 ! return to caller
	585	mov %t_kstack, %sp ! jump onto new stack
	586
	587	#define twin_tmp1 l4
	588	#define glob_tmp g4
	589	#define curptr g6
	590	ret_trap_entry:
	591	wr %t_psr, 0x0, %psr ! enable nesting again, clear ET
	592
	593	/* Will the rett land us in the invalid window? */
	594	mov 2, %g1
	595	sll %g1, %t_psr, %g1
	596
6d0f6bcf	597	set CONFIG_SYS_SPARC_NWINDOWS, %g2 !NWINDOWS
1e9a164e DH	598
	599	srl %g1, %g2, %g2
	600	or %g1, %g2, %g1
	601	rd %wim, %g2
	602	andcc %g2, %g1, %g0
	603	be 1f ! Nope, just return from the trap
	604	sll %g2, 0x1, %g1
	605
	606	/* We have to grab a window before returning. */
6d0f6bcf	607	set (CONFIG_SYS_SPARC_NWINDOWS-1), %g3 !NWINDOWS-1
1e9a164e DH	608
	609	srl %g2, %g3, %g2
	610	or %g1, %g2, %g1
	611	and %g1, 0xff, %g1
	612
	613	wr %g1, 0x0, %wim
	614
	615	/* Grrr, make sure we load from the right %sp... */
	616	PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)
	617
	618	restore %g0, %g0, %g0
	619	RW_LOAD(sp)
	620	b 2f
	621	save %g0, %g0, %g0
	622
	623	/* Reload the entire frame in case this is from a
	624	* kernel system call or whatever...
	625	*/
	626	1:
	627	PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)
	628	2:
	629	wr %t_psr, 0x0, %psr
	630	nop;
	631	nop;
	632	nop
	633
	634	jmp %t_pc
	635	rett %t_npc
	636
	637	/* This is called from relocated C-code.
	638	* It resets the system by jumping to _start
	639	*/
	640	_reset_reloc:
	641	set start, %l0
	642	call %l0
	643	nop