[people/ms/u-boot.git] / cpu / leon2 / start.S

/* This is where the SPARC/LEON3 starts
 * Copyright (C) 2007,
 * Daniel Hellstrom, daniel@gaisler.com
 *
 * See file CREDITS for list of people who contributed to this
 * project.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 */

#include <config.h>
#include <asm/asmmacro.h>
#include <asm/winmacro.h>
#include <asm/psr.h>
#include <asm/stack.h>
#include <asm/leon.h>
#include <timestamp.h>
#include <version.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

#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
/*
 * Version string
 */

	.data
	.globl	version_string
version_string:
	.ascii U_BOOT_VERSION
	.ascii " (", U_BOOT_DATE, " - ", U_BOOT_TIME, ")"
	.ascii CONFIG_IDENT_STRING, "\0"

	.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

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

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

/* add offsets to GOT table */
fixup_got:
	set	__got_start,%g4
	set	__got_end,%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
	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
	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
 */

	set	CONFIG_SYS_RELOC_MONITOR_BASE, %g2
	wr	%g0, %g2, %tbr

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

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