#include <asm.h>
#include <regdef.h>
#include <os.h>
#include <R4300.h>

LOCALSZ=	24
FRAMESZ=	(((NARGSAVE+LOCALSZ)*SZREG)+ALSZ)&ALMASK

RAOFF=		FRAMESZ-(1*SZREG)
S8OFF=		FRAMESZ-(2*SZREG)
GPOFF=		FRAMESZ-(3*SZREG)
S7OFF=		FRAMESZ-(4*SZREG)
S6OFF=		FRAMESZ-(5*SZREG)
S5OFF=		FRAMESZ-(6*SZREG)
S4OFF=		FRAMESZ-(7*SZREG)
S3OFF=		FRAMESZ-(8*SZREG)
S2OFF=		FRAMESZ-(9*SZREG)
S1OFF=		FRAMESZ-(10*SZREG)
S0OFF=		FRAMESZ-(11*SZREG)

FP20OFF =	FRAMESZ-(12*SZREG)
FP21OFF =	FRAMESZ-(13*SZREG)
FP22OFF =	FRAMESZ-(14*SZREG)
FP23OFF =	FRAMESZ-(15*SZREG)
FP24OFF =	FRAMESZ-(16*SZREG)
FP25OFF =	FRAMESZ-(17*SZREG)
FP26OFF =	FRAMESZ-(18*SZREG)
FP27OFF =	FRAMESZ-(19*SZREG)
FP28OFF =	FRAMESZ-(20*SZREG)
FP29OFF =	FRAMESZ-(21*SZREG)
FP30OFF =	FRAMESZ-(22*SZREG)
FP31OFF =	FRAMESZ-(23*SZREG)

ATOFF=		FRAMESZ-(24*SZREG)
A0OFF=		FRAMESZ

/*
 * int yieldAndCheck(unsigned int);
 *
 * Dirty callee saved registers, call osYieldThread, and then check
 * that registers have been restored correctly.
 *
 */
NESTED(yieldAndCheck, FRAMESZ, ra)
	subu	sp,FRAMESZ
	sw	ra,RAOFF(sp)
	sw	s8,S8OFF(sp)
	sw	gp,GPOFF(sp)
	sw	s7,S7OFF(sp)
	sw	s6,S6OFF(sp)
	sw	s5,S5OFF(sp)
	sw	s4,S4OFF(sp)
	sw	s3,S3OFF(sp)
	sw	s2,S2OFF(sp)
	sw	s1,S1OFF(sp)
	sw	s0,S0OFF(sp)
	sw	a0,A0OFF(sp)

	addi	s0,a0,0
	addi	s1,a0,1
	addi	s2,a0,2
	addi	s3,a0,3
	addi	s4,a0,4
	addi	s5,a0,5
	addi	s6,a0,6
	addi	s7,a0,7
	addi	gp,a0,8
	addi	s8,a0,9

	jal	osYieldThread

	lw	a0,A0OFF(sp)
	addi	t0,a0,0
	bne	s0,t0,bad0
	addi	t0,a0,1
	bne	s1,t0,bad0
	addi	t0,a0,2
	bne	s2,t0,bad0
	addi	t0,a0,3
	bne	s3,t0,bad0
	addi	t0,a0,4
	bne	s4,t0,bad0
	addi	t0,a0,5
	bne	s5,t0,bad0
	addi	t0,a0,6
	bne	s6,t0,bad0
	addi	t0,a0,7
	bne	s7,t0,bad0
	addi	t0,a0,8
	bne	gp,t0,bad0
	addi	t0,a0,9
	bne	s8,t0,bad0
	move	v0,zero
	b	common0
bad0:
	li	v0,-1
common0:
	lw	s0, S0OFF(sp)
	lw	s1, S1OFF(sp)
	lw	s2, S2OFF(sp)
	lw	s3, S3OFF(sp)
	lw	s4, S4OFF(sp)
	lw	s5, S5OFF(sp)
	lw	s6, S6OFF(sp)
	lw	s7, S7OFF(sp)
	lw	gp, GPOFF(sp)
	lw	s8, S8OFF(sp)
	lw	ra, RAOFF(sp)
	addu	sp, FRAMESZ
	j	ra
	END(yieldAndCheck)

	.set	noat
/*
 * int dirtyAndCheck(unsigned int);
 *
 * Dirty all registers and check them.
 *
 */
LEAF(dirtyAndCheck)
	subu	sp,FRAMESZ
	sw	ra,RAOFF(sp)
	sw	s8,S8OFF(sp)
	sw	gp,GPOFF(sp)
	sw	s7,S7OFF(sp)
	sw	s6,S6OFF(sp)
	sw	s5,S5OFF(sp)
	sw	s4,S4OFF(sp)
	sw	s3,S3OFF(sp)
	sw	s2,S2OFF(sp)
	sw	s1,S1OFF(sp)
	sw	s0,S0OFF(sp)
	mfc1	t0,$f20
	sw	t0,FP20OFF(sp)
	mfc1	t0,$f21
	sw	t0,FP21OFF(sp)
	mfc1	t0,$f22
	sw	t0,FP22OFF(sp)
	mfc1	t0,$f23
	sw	t0,FP23OFF(sp)
	mfc1	t0,$f24
	sw	t0,FP24OFF(sp)
	mfc1	t0,$f25
	sw	t0,FP25OFF(sp)
	mfc1	t0,$f26
	sw	t0,FP26OFF(sp)
	mfc1	t0,$f27
	sw	t0,FP27OFF(sp)
	mfc1	t0,$f28
	sw	t0,FP28OFF(sp)
	mfc1	t0,$f29
	sw	t0,FP29OFF(sp)
	mfc1	t0,$f30
	sw	t0,FP30OFF(sp)
	mfc1	t0,$f31
	sw	t0,FP31OFF(sp)
	sw	a0,A0OFF(sp)

	/*
	 * Set each CPU to a unique value.
	 */
	addi	AT,a0,1
	addi	v0,AT,1
	addi	v1,v0,1
	addi	a1,v1,1
	addi	a2,a1,1
	addi	a3,a2,1
	addi	t0,a3,1
	addi	t1,t0,1
	addi	t2,t1,1
	addi	t3,t2,1
	addi	t4,t3,1
	addi	t5,t4,1
	addi	t6,t5,1
	addi	t7,t6,1
	addi	s0,t7,1
	addi	s1,s0,1
	addi	s2,s1,1
	addi	s3,s2,1
	addi	s4,s3,1
	addi	s5,s4,1
	addi	s6,s5,1
	addi	s7,s6,1
	addi	t8,s7,1
	addi	t9,t8,1
	addi	gp,t9,1
	addi	s8,gp,1
	addi	ra,s8,1

	/*
	 * Set each floating point register to a unique value.
	 */
	mtc1	a0,$f0
	addi	a0,1
	mtc1	a0,$f1
	addi	a0,1
	mtc1	a0,$f2
	addi	a0,1
	mtc1	a0,$f3
	addi	a0,1
	mtc1	a0,$f4
	addi	a0,1
	mtc1	a0,$f5
	addi	a0,1
	mtc1	a0,$f6
	addi	a0,1
	mtc1	a0,$f7
	addi	a0,1
	mtc1	a0,$f8
	addi	a0,1
	mtc1	a0,$f9
	addi	a0,1
	mtc1	a0,$f10
	addi	a0,1
	mtc1	a0,$f11
	addi	a0,1
	mtc1	a0,$f12
	addi	a0,1
	mtc1	a0,$f13
	addi	a0,1
	mtc1	a0,$f14
	addi	a0,1
	mtc1	a0,$f15
	addi	a0,1
	mtc1	a0,$f16
	addi	a0,1
	mtc1	a0,$f17
	addi	a0,1
	mtc1	a0,$f18
	addi	a0,1
	mtc1	a0,$f19
	addi	a0,1
	mtc1	a0,$f20
	addi	a0,1
	mtc1	a0,$f21
	addi	a0,1
	mtc1	a0,$f22
	addi	a0,1
	mtc1	a0,$f23
	addi	a0,1
	mtc1	a0,$f24
	addi	a0,1
	mtc1	a0,$f25
	addi	a0,1
	mtc1	a0,$f26
	addi	a0,1
	mtc1	a0,$f27
	addi	a0,1
	mtc1	a0,$f28
	addi	a0,1
	mtc1	a0,$f29
	addi	a0,1
	mtc1	a0,$f30
	addi	a0,1
	mtc1	a0,$f31
	addi	a0,1

	li	a0,0x01000000
	ctc1	a0,$31

	/*
	 * Enable SW2 interrupt, which will allow a higher
	 * priority thread to be scheduled and dirty the registers.
	 */
	.set	noreorder
	mfc0	a0, C0_SR
	ori	a0, OS_IM_SW2
	mtc0	a0, C0_SR
	lw	a0,A0OFF(sp)
	.set	reorder

	/*
	 * Check that a0 was the value just re-loaded before the interrupt.
	 * If the interrupt occurs before the re-loading of a0 above,
	 * the test below is kind of useless.  It appears, however, that
	 * interrupt occurs several instructions after the mtc0.
	 */
	sw	AT,ATOFF(sp)
	lw	AT,A0OFF(sp)
	bne	a0,AT,bad1
	lw	AT,ATOFF(sp)

	/* 
	 * Now use a0 as temporary register to compare against all
	 * other CPU registers.
	*/
	lw	a0,A0OFF(sp)
	addi	a0,1
	bne	AT,a0,bad1
	addi	a0,1
	bne	v0,a0,bad1
	addi	a0,1
	bne	v1,a0,bad1
	addi	a0,1
	bne	a1,a0,bad1
	addi	a0,1
	bne	a2,a0,bad1
	addi	a0,1
	bne	a3,a0,bad1
	addi	a0,1
	bne	t0,a0,bad1
	addi	a0,1
	bne	t1,a0,bad1
	addi	a0,1
	bne	t2,a0,bad1
	addi	a0,1
	bne	t3,a0,bad1
	addi	a0,1
	bne	t4,a0,bad1
	addi	a0,1
	bne	t5,a0,bad1
	addi	a0,1
	bne	t6,a0,bad1
	addi	a0,1
	bne	t7,a0,bad1
	addi	a0,1
	bne	s0,a0,bad1
	addi	a0,1
	bne	s1,a0,bad1
	addi	a0,1
	bne	s2,a0,bad1
	addi	a0,1
	bne	s3,a0,bad1
	addi	a0,1
	bne	s4,a0,bad1
	addi	a0,1
	bne	s5,a0,bad1
	addi	a0,1
	bne	s6,a0,bad1
	addi	a0,1
	bne	s7,a0,bad1
	addi	a0,1
	bne	t8,a0,bad1
	addi	a0,1
	bne	t9,a0,bad1
	addi	a0,1
	bne	gp,a0,bad1
	addi	a0,1
	bne	s8,a0,bad1
	addi	a0,1
	bne	ra,a0,bad1

	/*
	 * Now check floating point registers
	 */
	lw	a0,A0OFF(sp)
	mfc1	t0,$f0
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f1
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f2
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f3
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f4
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f5
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f6
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f7
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f8
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f9
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f10
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f11
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f12
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f13
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f14
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f15
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f16
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f17
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f18
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f19
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f20
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f21
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f22
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f23
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f24
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f25
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f26
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f27
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f28
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f29
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f30
	bne	a0,t0,bad1
	addi	a0,1
	mfc1	t0,$f31
	bne	a0,t0,bad1

	li	a0,0x01000000
	cfc1	t0,$31
	bne	a0,t0,bad1

	move	v0,zero
	b	common1
bad1:
	li	v0,-1
common1:
	lw	t0,FP20OFF(sp)
	mtc1	t0,$f20
	lw	t0,FP21OFF(sp)
	mtc1	t0,$f21
	lw	t0,FP22OFF(sp)
	mtc1	t0,$f22
	lw	t0,FP23OFF(sp)
	mtc1	t0,$f23
	lw	t0,FP24OFF(sp)
	mtc1	t0,$f24
	lw	t0,FP25OFF(sp)
	mtc1	t0,$f25
	lw	t0,FP26OFF(sp)
	mtc1	t0,$f26
	lw	t0,FP27OFF(sp)
	mtc1	t0,$f27
	lw	t0,FP28OFF(sp)
	mtc1	t0,$f28
	lw	t0,FP29OFF(sp)
	mtc1	t0,$f29
	lw	t0,FP30OFF(sp)
	mtc1	t0,$f30
	lw	t0,FP31OFF(sp)
	mtc1	t0,$f31
	lw	s0,S0OFF(sp)
	lw	s1,S1OFF(sp)
	lw	s2,S2OFF(sp)
	lw	s3,S3OFF(sp)
	lw	s4,S4OFF(sp)
	lw	s5,S5OFF(sp)
	lw	s6,S6OFF(sp)
	lw	s7,S7OFF(sp)
	lw	gp,GPOFF(sp)
	lw	s8,S8OFF(sp)
	lw	ra,RAOFF(sp)
	addu	sp,FRAMESZ
	j	ra
END(dirtyAndCheck)

/*
 * void waitAndDirty(unsigned int);
 *
 * Block for interrupt message; then dirty all registers.
 *
 */

	.extern	softintMesgQueue

LEAF(waitAndDirty)
	subu	sp,FRAMESZ
	sw	ra,RAOFF(sp)
	sw	s8,S8OFF(sp)
	sw	gp,GPOFF(sp)
	sw	s7,S7OFF(sp)
	sw	s6,S6OFF(sp)
	sw	s5,S5OFF(sp)
	sw	s4,S4OFF(sp)
	sw	s3,S3OFF(sp)
	sw	s2,S2OFF(sp)
	sw	s1,S1OFF(sp)
	sw	s0,S0OFF(sp)
	mfc1	t0,$f20
	sw	t0,FP20OFF(sp)
	mfc1	t0,$f21
	sw	t0,FP21OFF(sp)
	mfc1	t0,$f22
	sw	t0,FP22OFF(sp)
	mfc1	t0,$f23
	sw	t0,FP23OFF(sp)
	mfc1	t0,$f24
	sw	t0,FP24OFF(sp)
	mfc1	t0,$f25
	sw	t0,FP25OFF(sp)
	mfc1	t0,$f26
	sw	t0,FP26OFF(sp)
	mfc1	t0,$f27
	sw	t0,FP27OFF(sp)
	mfc1	t0,$f28
	sw	t0,FP28OFF(sp)
	mfc1	t0,$f29
	sw	t0,FP29OFF(sp)
	mfc1	t0,$f30
	sw	t0,FP30OFF(sp)
	mfc1	t0,$f31
	sw	t0,FP31OFF(sp)
	sw	a0,A0OFF(sp)

	/*
	 * (void)osRecvMesg(&softintMesgQueue, OS_MESG_BLOCK);
	 */
	la	a0,softintMesgQueue
	li	a1,OS_MESG_BLOCK
	jal	osRecvMesg

	/*
	 * Set each CPU register to a unique value.
	 */
	lw	a0,A0OFF(sp)
	addi	AT,a0,1
	addi	v0,AT,1
	addi	v1,v0,1
	addi	a1,v1,1
	addi	a2,a1,1
	addi	a3,a2,1
	addi	t0,a3,1
	addi	t1,t0,1
	addi	t2,t1,1
	addi	t3,t2,1
	addi	t4,t3,1
	addi	t5,t4,1
	addi	t6,t5,1
	addi	t7,t6,1
	addi	s0,t7,1
	addi	s1,s0,1
	addi	s2,s1,1
	addi	s3,s2,1
	addi	s4,s3,1
	addi	s5,s4,1
	addi	s6,s5,1
	addi	s7,s6,1
	addi	t8,s7,1
	addi	t9,t8,1
	addi	gp,t9,1
	addi	s8,gp,1
	addi	ra,s8,1

	/*
	 * Set each FPU register to a unique value.
	 */
	mtc1	a0,$f0
	addi	a0,1
	mtc1	a0,$f1
	addi	a0,1
	mtc1	a0,$f2
	addi	a0,1
	mtc1	a0,$f3
	addi	a0,1
	mtc1	a0,$f4
	addi	a0,1
	mtc1	a0,$f5
	addi	a0,1
	mtc1	a0,$f6
	addi	a0,1
	mtc1	a0,$f7
	addi	a0,1
	mtc1	a0,$f8
	addi	a0,1
	mtc1	a0,$f9
	addi	a0,1
	mtc1	a0,$f10
	addi	a0,1
	mtc1	a0,$f11
	addi	a0,1
	mtc1	a0,$f12
	addi	a0,1
	mtc1	a0,$f13
	addi	a0,1
	mtc1	a0,$f14
	addi	a0,1
	mtc1	a0,$f15
	addi	a0,1
	mtc1	a0,$f16
	addi	a0,1
	mtc1	a0,$f17
	addi	a0,1
	mtc1	a0,$f18
	addi	a0,1
	mtc1	a0,$f19
	addi	a0,1
	mtc1	a0,$f20
	addi	a0,1
	mtc1	a0,$f21
	addi	a0,1
	mtc1	a0,$f22
	addi	a0,1
	mtc1	a0,$f23
	addi	a0,1
	mtc1	a0,$f24
	addi	a0,1
	mtc1	a0,$f25
	addi	a0,1
	mtc1	a0,$f26
	addi	a0,1
	mtc1	a0,$f27
	addi	a0,1
	mtc1	a0,$f28
	addi	a0,1
	mtc1	a0,$f29
	addi	a0,1
	mtc1	a0,$f30
	addi	a0,1
	mtc1	a0,$f31

	move	v0,zero
	lw	t0,FP20OFF(sp)
	mtc1	t0,$f20
	lw	t0,FP21OFF(sp)
	mtc1	t0,$f21
	lw	t0,FP22OFF(sp)
	mtc1	t0,$f22
	lw	t0,FP23OFF(sp)
	mtc1	t0,$f23
	lw	t0,FP24OFF(sp)
	mtc1	t0,$f24
	lw	t0,FP25OFF(sp)
	mtc1	t0,$f25
	lw	t0,FP26OFF(sp)
	mtc1	t0,$f26
	lw	t0,FP27OFF(sp)
	mtc1	t0,$f27
	lw	t0,FP28OFF(sp)
	mtc1	t0,$f28
	lw	t0,FP29OFF(sp)
	mtc1	t0,$f29
	lw	t0,FP30OFF(sp)
	mtc1	t0,$f30
	lw	t0,FP31OFF(sp)
	mtc1	t0,$f31
	lw	s0,S0OFF(sp)
	lw	s1,S1OFF(sp)
	lw	s2,S2OFF(sp)
	lw	s3,S3OFF(sp)
	lw	s4,S4OFF(sp)
	lw	s5,S5OFF(sp)
	lw	s6,S6OFF(sp)
	lw	s7,S7OFF(sp)
	lw	gp,GPOFF(sp)
	lw	s8,S8OFF(sp)
	lw	ra,RAOFF(sp)
	addu	sp,FRAMESZ
	j	ra
END(dirtyRegs)
	.set	at

LEAF(doBreak)
	break	0
	j	ra
END(doBreak)