locore.s

来自「操作系统SunOS 4.1.3版本的源码」· S 代码 · 共 1,251 行 · 第 1/2 页

/*	@(#)locore.s 1.1 92/07/30 SMI	*/

/*
 * Copyright (c) 1986 by Sun Microsystems, Inc.
 */

#include "assym.s"
#include "asm_linkage.h"
#include <sys/errno.h>
#include <sys/param.h>
#include <sun4/mmu.h>
#include <sun4/psl.h>
#include <sun4/pte.h>
#include <sun4/enable.h>
#include <sun4/cpu.h>
#include <sun4/trap.h>
#include "../../debug/debug.h"

/*
 * The debug stack. This must be the first thing in the data
 * segment (other than an sccs string) so that we don't stomp
 * on anything important. We get a red zone below this stack
 * for free when the text is write protected.
 */
#define	STACK_SIZE	0x8000
	.seg	"data"
	.global _estack
	.skip	STACK_SIZE
_estack:				! end (top) of debugger stack
	.skip	STACK_SIZE*2		! paranoid
_ekadbstack:
fpuregs:
	.skip	34*4			! %f0 - %f31, %fsr, plus shim
	.seg	"text"

#define DVMA	0xfff00000

#define SAVE_WINDOW(SBP) \
	st	%l0, [SBP + (0*4)]; \
	st	%l1, [SBP + (1*4)]; \
	st	%l2, [SBP + (2*4)]; \
	st	%l3, [SBP + (3*4)]; \
	st	%l4, [SBP + (4*4)]; \
	st	%l5, [SBP + (5*4)]; \
	st	%l6, [SBP + (6*4)]; \
	st	%l7, [SBP + (7*4)]; \
	st	%i0, [SBP + (8*4)]; \
	st	%i1, [SBP + (9*4)]; \
	st	%i2, [SBP + (10*4)]; \
	st	%i3, [SBP + (11*4)]; \
	st	%i4, [SBP + (12*4)]; \
	st	%i5, [SBP + (13*4)]; \
	st	%i6, [SBP + (14*4)]; \
	st	%i7, [SBP + (15*4)]

#define RESTORE_WINDOW(SBP) \
	ld	[SBP + (0*4)], %l0; \
	ld	[SBP + (1*4)], %l1; \
	ld	[SBP + (2*4)], %l2; \
	ld	[SBP + (3*4)], %l3; \
	ld	[SBP + (4*4)], %l4; \
	ld	[SBP + (5*4)], %l5; \
	ld	[SBP + (6*4)], %l6; \
	ld	[SBP + (7*4)], %l7; \
	ld	[SBP + (8*4)], %i0; \
	ld	[SBP + (9*4)], %i1; \
	ld	[SBP + (10*4)], %i2; \
	ld	[SBP + (11*4)], %i3; \
	ld	[SBP + (12*4)], %i4; \
	ld	[SBP + (13*4)], %i5; \
	ld	[SBP + (14*4)], %i6; \
	ld	[SBP + (15*4)], %i7

/*
 * Trap vector macros.
 */
#define TRAP(H) \
	b (H); mov %psr,%l0; nop; nop;

#define WIN_TRAP(H) \
        mov %psr,%l0; mov %wim,%l3; b (H); mov 7,%l6;

#define SYS_TRAP(T) \
        mov %psr,%l0; mov (T),%l4; b sys_trap; mov 7,%l6;

#define BAD_TRAP	SYS_TRAP(_fault);
#define NO_OP		0x01000000

/*
 * Trap vector table.
 * This must be the first text in the boot image.
 *
 * When a trap is taken, we vector to DEBUGSTART+(TT*16) and we have
 * the following state:
 *	2) traps are disabled
 *	3) the previous state of PSR_S is in PSR_PS
 *	4) the CWP has been decremented into the trap window
 *	5) the previous pc and npc is in %l1 and %l2 respectively.
 *
 * Registers:
 *	%l0 - %psr immediately after trap
 *	%l1 - trapped pc
 *	%l2 - trapped npc
 *	%l3 - trap handler pointer (sys_trap only)
 *	%l6 - NW-1, for wim calculations
 *
 * Note: DEBUGGER receives control at vector 0 (trap).
 */
	.seg	"text"
	.align 4

	.global _start, _scb
_start:
_scb:
	TRAP(enter);				! 00
	BAD_TRAP;				! 01 text fault
	BAD_TRAP;				! 02 unimp instruction
	BAD_TRAP;				! 03 priv intstruction
	TRAP(_fp_disabled);			! 04 fp disabled
	WIN_TRAP(_window_overflow);		! 05
	WIN_TRAP(_window_underflow);		! 06
	BAD_TRAP;				! 07 alignment
	BAD_TRAP;				! 08 fp exception
	SYS_TRAP(0x9);				! 09 data fault
	BAD_TRAP;				! 0A tag_overflow
	BAD_TRAP; BAD_TRAP;			! 0B - 0C
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 0D - 10
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 11 - 14 int 1-4
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 15 - 18 int 5-8
	BAD_TRAP; 				! 19 int 13
	BAD_TRAP; 				! 1A int 14
	BAD_TRAP; BAD_TRAP; 			! 1B - 1C int 9-12
	BAD_TRAP; 				! 1D int 13
	BAD_TRAP;				! 1E int 14
	SYS_TRAP(0x31);				! 1F int 15
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! 20 - 23
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! 24 - 27
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! 28 - 2B
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! 2C - 2F
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! 30 - 34
	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 - 44
	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
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! 60 - 64
	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 - 74
	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
	!
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 80 - 83
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 84 - 87
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 88 - 8B
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 8C - 8F
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 90 - 93
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 94 - 97
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 98 - 9B
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 9C - 9F
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! A0 - A3
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! A4 - A7
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! A8 - AB
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! AC - AF
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! B0 - B3
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! B4 - B7
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! B8 - BB
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! BC - BF
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! C0 - C3
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! C4 - C7
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! C8 - CB
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! CC - CF
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! D0 - D3
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! D4 - D7
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! D8 - DB
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! DC - DF
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! E0 - E3
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! E4 - E7
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! E8 - EB
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! EC - EF
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! F0 - F3
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! F4 - F7
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! F8 - FB
/*
	BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	! FC - FF
*/
	BAD_TRAP; BAD_TRAP; TRAP(_trap); TRAP(_trap);	! FC - FF

/*
 * Debugger vector table.
 * Must follow trap table.
 */
	b,a	enter			! dv_entry
	.word	_trap			! dv_trap
	.word	_pagesused		! dv_pages
	.word	_scbsync		! dv_scbsync
	.word	0

/*
 * Debugger entry point.
 * First we must figure out if we are running in correctly adjusted
 * addresses. If so, we must have been called by debugged code
 * and thus we just want to call the command interpreter. If not,
 * then we continue to run in the wrong adddress. All of the code
 * must be carefully written to be position independent code, since
 * we are linked for running out of high addresses, but we get control
 * running in low addresses. We run off the stack set up by the caller.
 */
enter:
	mov	%o7, %o0		! save o7
	call	lea			! load the pc (XXX change for JALR)
	nop
lea:	mov	%o7, %g1
	mov	%o0, %o7		! restore o7
	set	(lea-_start-8), %o0	! compute real address of start
	sub	%g1, %o0, %o0
	set	_start, %g1
	cmp	%g1, %o0		! compare to see if virtual == real
	bne	init			! ==, we have been relocated already
	nop
	!
	! Enter debugger by doing a software trap.
	! We ASSUME that the software trap we have set up is still there.
	!
	t	TRAPBRKNO-1
	nop
	retl
	nop

	!
	! We have not been relocated yet.
	! First, find the lowest pmeg used between MONSTART and MONEND.
	!
init:
	mov	%psr, %g1
	bclr	PSR_PIL, %g1		! PIL = 15
	bset	(15 << 8), %g1
	mov	%g1, %psr
	nop;nop;nop
	save	%sp, -MINFRAME, %sp	! get new window
	set	CONTEXT_REG, %l0	! get into context 0
	stba	%g0, [%l0]ASI_CTL
	set	MONSTART, %l0
	set	MONEND, %l1
	set     ID_PROM, %g1
	clr	%o0
	add	%g1, 0x1, %g1
	lduba	[%g1]ASI_CTL, %o0	! read machine type

	cmp	%o0, CPU_SUN4_470	! sunray
	be	1f
	mov	NPMGRPS_470, %l2

	cmp	%o0, CPU_SUN4_330	! stingray
	be	1f
	mov	NPMGRPS_330, %l2

	cmp	%o0, CPU_SUN4_260	! sunrise
	be	1f
	mov	NPMGRPS_260, %l2

	cmp	%o0, CPU_SUN4_110	! cobra
	be	1f
	mov	NPMGRPS_110, %l2

	mov	NPMGRPS_260, %l2	! if not known default to sunrise
1:
	mov	%l2, %g3		! save npmegs for saving later
	sub	%l2, 1, %l2		! create last pmeg number
	mov	%l2, %l5		! and segmask
	set	PMGRPSIZE, %l3
	mov	%sp, %g6
	mov	%fp, %g5
1:
	lduha	[%l0]ASI_SM, %g1
	and	%g1, %l5, %g1
	cmp	%l2, %g1
	bgu,a	2f
	mov	%g1, %l2
2:
	add	%l0, %l3, %l0
	cmp	%l0, %l1
	blu	1b
	nop

	!
	! Now check between DVMA and the end of the address space.
	!
	set	DVMA, %l0
1:
	lduha	[%l0]ASI_SM, %g1
	andcc	%g1, %l5, %g1
	bne	2f
	nop
	! A cobra booted from the net will have a pmeg mapped as zero.
	! Mark it unused; otherwise it causes trouble.
	stha	%l5, [%l0]ASI_SM
	ba	3f
	nop
2:
	cmp	%l2, %g1
	bgu,a	3f
	mov	%g1, %l2
3:
	addcc	%l0, %l3, %l0
	bnz	1b
	nop
	!
	! We have the lowest used pmeg.
	! Subtract the amount we are going to use.
	!
	set	_start, %l0
	set	_end, %l1
	set	PMGRPOFFSET, %g1
	add	%l1, %g1, %l4		! round up end to next segment boundary
	andn	%l4, %g1, %l4
	andn	%l0, %g1, %l3		! round down start to segment boundary
	sub	%l4, %l3, %l5		! compute difference
	srl	%l5, PMGRPSHIFT, %l5	! convert to segments
	sub	%l2, %l5, %l2		! compute first pmeg to use
	mov	%l2, %l7		! save it 
	!
	! setup segment map
	!
	set	NBPG, %g2
1:
	stha	%l2, [%l3]ASI_SM	! setup pmeg
2:					! invalidate pages in the pmeg
	sta	%g0, [%l3]ASI_PM	! invalidate pme
	add	%l3, %g2, %l3
	btst	%g1, %l3		! at next segment?
	bnz	2b
	nop

	cmp	%l3, %l4		! at end?
	blu,a	1b
	add	%l2, 1, %l2		! compute next pmeg
	!
	! Find physical pages to use.
	!
	set	PGOFSET, %g1
	add	%l1, %g1, %l4		! round up end to next page boundary
	andn	%l4, %g1, %l4
	andn	%l0, %g1, %l3		! round down start to page boundary
	sub	%l4, %l3, %l6		! compute difference
	srl	%l6, PGSHIFT, %l6	! convert to pages
#ifdef SAS
	set	(1024*1024)>>PGSHIFT, %l2
#else
	sethi	%hi(ROMP_MEMAVAIL), %l2	! find memory available
	ld	[%l2 + %lo(ROMP_MEMAVAIL)], %l2
	nop
	ld	[%l2], %l2
	nop
	srl	%l2, PGSHIFT, %l2	! convert to pages
#endif
	sub	%l2, %l6, %l2		! compute first page to use
	!
	! setup page map
	!
	sethi	%hi(PG_V | PG_KW), %g1
	add	%g1, %l2, %g1
1:
	sta	%g1, [%l3]ASI_PM
	add	%l3, %g2, %l3		! next virtual page
	cmp	%l3, %l4
	blu,a	1b
	add	%g1, 1, %g1		! delay slot, next phys page

	!
	! Copy program to correct address.
	!
	set	_start, %o0		! must be word alinged, they are inst
	set	_edata, %o1
	sub	%o1, %o0, %o2		! size of text and data
	set	_end, %o4
	sub	%o4, %o1, %o5		! size of bss
2:
	ld	[%i0], %o3		! relocate
	inc	4, %i0			
	st	%o3, [%o0]
	deccc	4, %o2
	bg	2b
	inc	4, %o0
3:
	deccc	4, %o5			! zero bss
	st	%g0, [%o1]
	bg	3b
	inc	4, %o1
	
	!
	! Jump to relocated code.
	!
	set	1f, %g1			! non PC-relative branch
	jmp	%g1
	nop
1:
	!
	! PHEW! Now we are running with correct addresses
	! and can use non-position independent code.
	! fix implementation dependent parameters now
	!
	call	fiximp
	nop

	!
	! Save some of the memory numbers we found during setup.
	!
	set     _npmgrps, %g4
	st      %g3, [%g4]              ! store in npmgrps
	sub	%g3, 1, %g3
	set     _segmask, %g4
	st      %g3, [%g4]              ! store in mask for getsegmap
	sethi	%hi(_lastpg), %g1
	st	%l2, [%g1 + %lo(_lastpg)]
	sethi	%hi(_lastpm), %g1
	st	%l7, [%g1 + %lo(_lastpm)]
	sethi	%hi(_pagesused), %g1
	st	%l6, [%g1 + %lo(_pagesused)]

	!
	! Save monitor's level14 clock interrupt vector code.
	!
	mov     %tbr, %l4               ! save monitor's tbr
	bclr    0xfff, %l4              ! remove tt
	or      %l4, TT(T_INT_LEVEL_14), %l4
	set     _scb, %l5
	or      %l5, TT(T_INT_LEVEL_14), %l5
	ld	[%l4 + 0*4], %o0
	ld	[%l4 + 1*4], %o1
	ld	[%l4 + 2*4], %o2
	ld	[%l4 + 3*4], %o3
	st	%o0, [%l5 + 0*4]
	st	%o1, [%l5 + 1*4]
	st	%o2, [%l5 + 2*4]
	st	%o3, [%l5 + 3*4]

	!
	! Save monitor's trap zero 
	!
	mov     %tbr, %l4               ! save monitor's tbr
	bclr    0xfff, %l4              ! remove tt
	or      %l4, 0x800, %l4
	set     _scb, %l5
	or      %l5, 0x800, %l5
	ld	[%l4 + 0*4], %o0
	ld	[%l4 + 1*4], %o1
	ld	[%l4 + 2*4], %o2
	ld	[%l4 + 3*4], %o3
	st	%o0, [%l5 + 0*4]
	st	%o1, [%l5 + 1*4]
	st	%o2, [%l5 + 2*4]
	st	%o3, [%l5 + 3*4]

	!
	! Call startup to do the rest of the startup work.
	!
	set	_scb, %g1		! setup kadb tbr
	mov 	%g1, %tbr
	mov	0x2, %wim
	mov 	%psr, %g1
	bclr 	PSR_CWP, %g1
	mov 	%g1, %psr
	nop; nop; nop;			! psr delay

	!
	! use our own stack instead of the callers now
	!
	set	_ekadbstack, %o0
	sub	%o0, (MINFRAME + REGSIZE), %sp
	call	_startup
	clr	%fp

	!
	! call main to enter the debugger
	!
	call	_main
	nop
	mov	%psr, %g1
	bclr	PSR_PIL, %g1		! PIL = 14
	bset	(14 << 8), %g1
	mov	%g1, %psr
	nop;nop;nop
	t	TRAPBRKNO-1
	nop
	ret 				! should not return
	restore

/*
 * exitto(addr)
 * int *addr;
 */
	ENTRY(_exitto)
	save	%sp, -MINFRAME, %sp
	jmpl	%i0, %o7		! register-indirect call
	nop
	ret
	restore

/*
 * This is where breakpoint traps go.
 * We assume we are in the normal condition after a trap.
 */
	ENTRY(trap)
	!
	! dump the whole cpu state (all windows) on the stack.
	!
	set	_regsave, %l3
	st	%l0, [%l3 + R_PSR]
	st	%l1, [%l3 + R_PC]
	st	%l2, [%l3 + R_NPC]
	mov	%wim, %l4
	st	%l4, [%l3 + R_WIM]
	mov	%g0, %wim		! zero wim so that we can move around
	mov	%tbr, %l4
	st	%l4, [%l3 + R_TBR]
	mov	%y, %l4
	st	%l4, [%l3 + R_Y]
	st	%g1, [%l3 + R_G1]
	st	%g2, [%l3 + R_G2]
	st	%g3, [%l3 + R_G3]
	st	%g4, [%l3 + R_G4]
	st	%g5, [%l3 + R_G5]
	st	%g6, [%l3 + R_G6]
	st	%g7, [%l3 + R_G7]
	add	%l3, R_WINDOW, %g7
	set	_nwindows, %g6
	ld	[%g6], %g6
	bclr	PSR_CWP, %l0		! go to window 0
	mov	%l0, %psr
	nop; nop; nop;			! psr delay
1:
	st	%l0, [%g7 + 0*4]	! save locals
	st	%l1, [%g7 + 1*4]
	st	%l2, [%g7 + 2*4]
	st	%l3, [%g7 + 3*4]
	st	%l4, [%g7 + 4*4]
	st	%l5, [%g7 + 5*4]
	st	%l6, [%g7 + 6*4]
	st	%l7, [%g7 + 7*4]
	st	%i0, [%g7 + 8*4]	! save ins
	st	%i1, [%g7 + 9*4]
	st	%i2, [%g7 + 10*4]
	st	%i3, [%g7 + 11*4]
	st	%i4, [%g7 + 12*4]
	st	%i5, [%g7 + 13*4]
	st	%i6, [%g7 + 14*4]
	st	%i7, [%g7 + 15*4]
	add	%g7, WINDOWSIZE, %g7
	subcc	%g6, 1, %g6		! all windows done?
	bnz	1b
	restore				! delay slot, increment CWP
	!
	! Back in window 0.
	!
	set	_regsave, %g2		! need to get back to state of entering
	ld	[%g2 + R_WIM], %g1
	mov	%g1, %wim
	ld	[%g2 + R_PSR], %g1
	bclr	PSR_PIL, %g1		! PIL = 14
	bset	(14 << 8), %g1
	mov     %g1, %psr               ! go back to orig window
	nop; nop; nop;
        !
        ! Now we must make sure all the window stuff goes to memory.
        ! Flush all register windows to the stack.
        ! But wait! If we trapped into the invalid window, we can't just
        ! save because we'll slip under the %wim tripwire.
        ! Do one restore first, so subsequent saves are guaranteed
        ! to flush the registers. (Don't worry about the restore
        ! triggering a window underflow, since if we're in a trap
        ! window the next window up has to be OK.)
        ! Do all this while still using the kernel %tbr: let it worry
        ! about user windows and user stack faults.
        !
	restore
        mov     %psr, %g1               ! get new CWP
        wr      %g1, PSR_ET, %psr       ! enable traps
        nop;nop;nop
        save    %sp, -SA(MINFRAME), %sp ! now we're back in the trap window
        mov     %sp, %g3
        mov     %fp, %g4
	set	_nwindows,%g6
	ld	[%g6], %g6
1:
        deccc   %g6                     ! all windows done?
        bnz     1b
        save    %sp, -WINDOWSIZE, %sp
	set	_nwindows,%g6
	ld	[%g6], %g6
2:
        deccc   %g6                     ! all windows done?
        bnz     2b
        restore                         ! delay slot, increment CWP
 
	mov	%psr, %g1
	wr	%g1, PSR_ET, %psr	! disable traps while working
	nop; nop; nop			! waiting...
	mov	2, %wim			! setup wim
	bclr	PSR_CWP, %g1		! go to window 0
	bclr	PSR_PIL, %g1		! PIL = 14
	bset	(14 << 8), %g1
        wr      %g1, PSR_ET, %psr       ! rewrite %psr, but keep traps disabled
        nop; nop; nop
        mov     %g3, %sp                ! put back %sp and %fp
        mov     %g4, %fp
        mov     %g1, %psr               ! now enable traps
        nop; nop; nop

	! save fpu
	sethi	%hi(_fpu_exists), %g1
	ld	[%g1 + %lo(_fpu_exists)], %g1
	tst	%g1			! don't bother if no fpu
	bz	1f
	nop
	set	_regsave, %l3		! was it on?