[people/ms/u-boot.git] / arch / sparc / cpu / leon2 / 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>

/* 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"
	.align 4

_hardreset:
1000:
	flush
	nop
	nop
	nop

	/* Init Cache */
	set     (LEON2_PREGS+LEON_REG_CACHECTRL_OFFSET), %g1
	set     0x0081000f, %g2
	st      %g2, [%g1]

	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

leon2_init:
	/* LEON2 Register Base in g1 */
	set	LEON2_PREGS, %g1

leon2_init_cache:
	/* Set Cache control register */
	set	0x1000f, %g2
	st	%g2, [%g1 + 0x14]

leon2_init_clear:

	/* Clear LEON2 registers */
	st	%g0, [%g1 + LEON2_ECTRL]
	st	%g0, [%g1 + LEON2_IMASK]
	st	%g0, [%g1 + LEON2_IPEND]
	st	%g0, [%g1 + LEON2_IFORCE]
	st	%g0, [%g1 + LEON2_ICLEAR]
	st	%g0, [%g1 + LEON2_IOREG]
	st	%g0, [%g1 + LEON2_IODIR]
	st	%g0, [%g1 + LEON2_IOICONF]
	st	%g0, [%g1 + LEON2_UCTRL0]
	st	%g0, [%g1 + LEON2_UCTRL1]

leon2_init_ioport:
	/* I/O port initialization */
	set	0xaa00, %g2
	st	%g2, [%g1 + LEON2_IOREG]

leon2_init_mctrl:

	/* memory config register 1 */
	set	CONFIG_SYS_GRLIB_MEMCFG1, %g2
	ld	[%g1], %g3		!
	and	%g3, 0x300, %g3
	or	%g2, %g3, %g2
	st	%g2, [%g1 + LEON2_MCFG1]
	set	CONFIG_SYS_GRLIB_MEMCFG2, %g2		! Load memory config register 2
#if !( defined(TSIM) || !defined(BZIMAGE))
	st	%g2, [%g1 + LEON2_MCFG2]	! only for prom version, else done by "dumon -i"
#endif
	set	CONFIG_SYS_GRLIB_MEMCFG3, %g2		! Init FT register
	st	%g2, [%g1 + LEON2_ECTRL]
	ld	[%g1 + LEON2_ECTRL], %g2
	srl	%g2, 30, %g2
	andcc	%g2, 3, %g6
	bne,a	leon2_init_wim
	 mov	%g0, %asr16		! clear err_reg

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

leon2_init_psr:
	set	0x1000, %g3
	mov	%psr, %g2
	wr	%g2, %g3, %psr
	nop
	nop
	nop

leon2_init_stackp:
	set	CONFIG_SYS_INIT_SP_OFFSET, %fp
	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

reloc:
	SPARC_LOAD_ADDRESS(TEXT_START, l7, g2)
	SPARC_LOAD_ADDRESS(DATA_END, l7, g3)
	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
	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)
 *
 */

	/* clear bss area (the relocated) */
clr_bss:
	SPARC_LOAD_ADDRESS(__bss_start, l7, g2)
	SPARC_LOAD_ADDRESS(__bss_end, l7, 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 */
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 */
fixup_got:
	SPARC_LOAD_ADDRESS(__got_start, l7, g4)
	add	%g4, %g6, %g4
	SPARC_LOAD_ADDRESS(__got_end, l7, g3)
	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

prom_relocate:
	SPARC_LOAD_ADDRESS(__prom_start, l7, g2)
	SPARC_LOAD_ADDRESS(__prom_end, l7, g3)
	/*
	 * Calculated addres is stored in this variable by
	 * reserve_prom() function in common/board_f.c
	 */
	SPARC_LOAD_ADDRESS(__prom_start_reloc, l7, g4)
	ld	[%g4], %g4

40:	ldd	[%g2], %l0
	ldd	[%g2+8], %l2
	std	%l0, [%g4]
	std	%l2, [%g4+8]
	inc	16, %g2
	cmp	%g2, %g3
	bcs	40b
	 inc	16, %g4

! %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
 */
update_trap_table_address:
	wr	%g0, %o2, %tbr
	nop
	nop
	nop

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

!------------------------------------------------------------------------------

/* 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

!------------------------------------------------------------------------------

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