/**************************************************************************
 *									  *
 *		 Copyright (C) 1994, Silicon Graphics, Inc.		  *
 *									  *
 *  These coded instructions, statements, and computer programs  contain  *
 *  unpublished  proprietary  information of Silicon Graphics, Inc., and  *
 *  are protected by Federal copyright law.  They  may  not be disclosed  *
 *  to  third  parties  or copied or duplicated in any form, in whole or  *
 *  in part, without the prior written consent of Silicon Graphics, Inc.  *
 *									  *
 **************************************************************************/

/**************************************************************************
 *
 *  Module: exceptasms.s
 *
 *  $Revision: 1.11 $
 *  $Date: 2003/11/20 01:34:05 $
 *  $Author: wheeler $
 *  $Source: /root/leakn64/depot/rf/sw/bbplayer/libultra/nintendo/exception/exceptasm.s,v $
 *
 *  Description:
 *      This file contains exception handling routines for Ultra 64 OS.
 *	When an exception of interest occurs, the exception handler sends
 *	a message to a message queue; both the message and the message queue
 *	must be	pre-registered via the osSetEventMesg routine.
 *
 *	The exception handler calls the dispatcher to generate a thread 
 *	context switch when the associated message queue contains a blocking 
 *	thread that has higher priority than the current running thread. 
 *	Otherwise, the interrupted thread should resume normal processing 
 *	after the completion of the exception handler. 
 *
 *	A running thread should only be preempted by a higher priority thread
 *	(waiting for the event message), but NOT by a thread with the
 *	 same priority.
 *
 *  Pseudo-code:
 *	save the registers into the context of the running thread
 *	read the cause register
 *	if not an interrupt, breakpoint, or coprocessor unusable exception, then
 *		jump to panic (spin forever since it''s a real exception)
 *	if an interrupt, then
 *		find the correct interrupt handler
 *		clear the interrupt, if necessary
 *	find which event message queue/message to load
 *	load the proper message queue/message from the interrupt message array
 *	insert message to message queue
 *	advance corresponding message queue counters (i.e. first, validCount)
 *	if (message queue is not full) and 
 *		(there is a blocking thread in message queue)
 *		dequeue blocking thread from message queue
 *		enqueue the blocking thread to run queue
 *		if (blocking thread''s priority > current thread''s priority) 
 *			enqueue the current thread to run queue behind other
 *			threads of same priority
 *		else
 *			enqueue the running thread to FRONT of run queue
 *		jump to dispatcher
 *	else
 *		enqueue the running thread to FRONT of run queue
 *		jump to dispatcher
 */

#include <asm.h>
#include <regdef.h>
#include <R4300.h>
#include <os.h>
#include <rcp.h>
#ifdef __sgi__
#include <task.h>
#endif
#include <rdb.h>
#ifdef BBPLAYER
#include <bcp.h>
#include <os_bb.h>
#include <bbdebug.h>
#endif

#include "exceptasm.h"
#include "threadasm.h"

#define		WORK_SP		0x60
#define 	R26_OFS		(0x50-WORK_SP)
#define 	R27_OFS		(0x58-WORK_SP)
#define 	PT_ENTRY	0xbff00014

	.set mips3
	.rdata

	/*
	 * Lookup table for offset into interrupt handler table below
	 */
__osIntOffTable:
	.byte 0*4, 5*4, 6*4, 6*4, 7*4, 7*4, 7*4, 7*4
	.byte 8*4, 8*4, 8*4 ,8*4, 8*4, 8*4, 8*4, 8*4
	.byte 0*4, 1*4, 2*4, 2*4, 3*4, 3*4, 3*4, 3*4
	.byte 4*4, 4*4, 4*4, 4*4, 4*4, 4*4, 4*4, 4*4

	/*
	 * Interrupt handler table, ordered in increasing order of priority
	 */
__osIntTable:
	.word	redispatch
	.word	sw1			# CAUSE_SW1
	.word	sw2			# CAUSE_SW2
	.word	rcp			# INT0 - CAUSE_IP3
	.word	cart			# INT1 - CAUSE_IP4
	.word	prenmi			# INT2 - CAUSE_IP5
	.word	IP6_Hdlr		# INT3 - CAUSE_IP6
	.word	IP7_Hdlr		# INT4 - CAUSE_IP7
	.word	counter			# CAUSE_IP8


#ifndef _FINALROM
.globl __osCauseTable_pt;
__osCauseTable_pt:
	.byte	0			#  0 :Int
	.byte	0			#  1 :Mod
	.byte	0			#  2 :TLBL
	.byte	0			#  3 :TLBS
	.byte	0			#  4 :AdEL
	.byte	0			#  5 :AdES
	.byte	0			#  6 :IBE
	.byte	0			#  7 :DBE
	.byte	0			#  8 :Sys
	.byte	1			#  9 :Bp
	.byte	0			# 10 :RI
	.byte	0			# 11 :CpU
	.byte	0			# 12 :Ov
	.byte	0			# 13 :Tr
	.byte	0			# 14 :VCEI
	.byte	0			# 15 :FPE
	.byte	1			# 16 :
	.byte	1			# 17 :
	.byte	1			# 18 :
	.byte	1			# 19 :
	.byte	1			# 20 :
	.byte	1			# 21 :
	.byte	1			# 22 :
	.byte	1			# 23 :WATCH
	.byte	1			# 24 :
	.byte	1			# 25 :
	.byte	1			# 26 :
	.byte	1			# 27 :
	.byte	1			# 28 :
	.byte	1			# 29 :
	.byte	1			# 30 :
	.byte	0			# 31 :VCED
#endif

	.data
	/*
	 * Optional HW interrupt support routines from game
	 */
.globl __osHwIntTable;
__osHwIntTable:
	.word	0			# INT0 - RCP (handler)
	.word	0			#            (stack)
	.word	0			# INT1 - Cart from AD16 (handler)
	.word	0			#                       (stack)
	.word	0			# INT2 - PRENMI from PIF (handler)
	.word	0			#                        (stack)
	.word	0			# INT3 - NC (rdb read on Indy dev board) (handler)
	.word	0			#                                        (stack)
	.word	0			# INT4 - NC (rdb write on Indy dev board) (handler)
	.word	0			#                                         (stack)

.globl __osPiIntTable;
__osPiIntTable:
	.word	0			# PI int (handler)
	.word	0			#        (stack)


#ifndef _FINALROM
/* RDB variables */	
__osRdb_DbgRead_Ct:		# decrements with each packet, when zero done.
	.word   0

__osRdb_Mesg:	
	.word   0		

__os_Kdebug_Pkt:	
	.word   0	

/* Thread Profiler */
__osPreviousThread:	
	.word	0
		
#endif /* ifndef _FINALROM */


	.text
	.set	noat
	.set	noreorder

LEAF(__osExceptionPreamble)
	la	k0,__osException
	jr	k0
	nop
END(__osExceptionPreamble)


#ifndef _FINALROM
LEAF(__ptExceptionPreamble)
	sw	k0,R26_OFS(sp)
	beq	$0,$0,pt_next
	lui	k0,%hi(__ptException)
	nop
	nop
pt_next:
	addiu	k0,k0,%lo(__ptException)
	jr	k0
	nop
END(__ptExceptionPreamble)

LEAF(__ptException)
	mfc0	k0,C0_CAUSE
	andi	k0,k0,0x7c
	bne	k0,$0,pt_not_Int
	srl	k0,k0,2
	mfc0	k0,C0_CAUSE
	nop
	andi	k0,k0,0x4000
	beq	k0,$0,__osException
	nop
	j	pt_break
	nop

pt_not_Int:
	sw	k1,R27_OFS(sp)
	la	k1,__osCauseTable_pt
	add	k1,k1,k0
	lbu	k0,0(k1)
	beq	k0,$0,__osException
	nop
pt_break:
pi_ok_loop:
	lui	k0,0xa460
	lw	k0,0x10(k0)
	nop
	andi	k0,k0,3
	bne	k0,$0,pi_ok_loop
	nop
	lui	k0,0xbff1
	lw	k0,-0x7ffc(k0)
	nop
	andi	k0,k0,8
	bne	k0,$0,pt_prof
	lui	k0,0xffff&(PT_ENTRY>>16)
	ori	k0,k0,0xffff&PT_ENTRY
	lw	k1,R27_OFS(sp)
	jr	k0
	addiu	sp,sp,-WORK_SP
pt_prof:
	mfc0	k0,C0_EPC
	lui	k1,0xbff1
	sw	k0,-0x8000(k1)
	lw	k1,R27_OFS(sp)
	lw	k0,R26_OFS(sp)
	eret
	nop

END(__ptException)
#endif

LEAF(__osException)
	/*
	 * Save registers into the context of a temporary thread structure
	 * Register k0 & k1 are reserved for kernel use. 
	 */

	la	k0,__osThreadSave  # load address of struct to save thread to

	/*
	 * Save AT first, so we can enable use of AT by the assembler early on.
	 */
	sd	AT,T_CONTEXT_AT(k0)

	.set	at

	/*
	 * Save the SR with the EXL bit ON, but then turn it off during the
	 * the exception handler (aids debugging.)  Turning off SR_IE will
	 * disable further interrupts.
	 *
	 * The SR contents will be accessible in k1 for the entire handler.
	 */
	mfc0	k1,C0_SR		# get the status register
	sw	k1,T_CONTEXT_SR(k0)	# save the status register
	and	k1,~(SR_EXL|SR_IE)	# turn off EXL and IE bits
	mtc0	k1,C0_SR		# write the new status to the sr

	/* save t0-t2 so that they can be used as scratch pad */
	sd	t0,T_CONTEXT_T0(k0)
	sd	t1,T_CONTEXT_T1(k0)
	sd	t2,T_CONTEXT_T2(k0)
	sw	zero,T_FP(k0)
	mfc0	t0,C0_CAUSE		# get the cause register and put it in t0

	.set	reorder

#ifndef _FINALROM
	/* 
	 * The following block of code, checks to see if there is an rdb interrupt
	 * and if there is it handles it. In most cases, that means sending or
	 * receiving the next data packet, and returning without doing a thread
	 * swap. In some cases however, you need to do a thread swap. In that case
	 * jump to savecontext, then send the message, and handle like any other
	 * interrupt.
	 */

        lw      t2,__kmc_pt_mode
	bne	t2,$0,skip_kmc_mode

	andi	t1,t0,CAUSE_EXCMASK	
	li	t2,EXC_INT
	bne	t1,t2,savecontext	# if no cause bits set, branch

#ifdef BBPLAYER
	/* check for bb ide interrupt on IP4 */
	lw	t1,__osBbIsBb
	beq	t1,zero,2f		# only on a real BBPLAYER
	and	t1,k1,t0		# and SR and CAUSE register
	and	t2,t1,CAUSE_IP4		# and with CAUSE_IP4
	beq	t2,zero,notIP7		# not debug interrupt
	lw	t1,PHYS_TO_K1(MI_EINTR_REG)
	and	t1,t1,MI_INTR_IDE
	beq	t1,zero,notIP7		# not debug interrupt
	/* ide interrupt, determine whether req or ack */
	lhu	t1,PHYS_TO_K1(IDE_RDB_STATUS_REG)
	and	t2,t1,IDE_RDB_STATUS_HOST_REQ
	beq	t2,zero,1f
	/* incoming request (equivalent to IP7) */
	la	t1,PHYS_TO_K1(IDE_RDB_DATA_REG)
	lhu	t2,0(t1)		# load 32-bit word into t0
	lhu	t0,2(t1)		# by reading 2 half-words
	sll	t2,t2,16
	or	t0,t0,t2
#if 0
	li	t1,IDE_RDB_CTRL_CLR_HOST_REQ|IDE_RDB_CTRL_SET_BB_ACK
	sh	t1,PHYS_TO_K1(IDE_RDB_CTRL_REG) # clear interrupt
	lw	t1,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
#endif
	b	rdb_req
1:
	and	t2,t1,IDE_RDB_STATUS_HOST_ACK /*XXXrtc hack*/
	beq	t2,zero,2f

	/* incoming ack, (equivalent to IP6) */
	li	t1,IDE_RDB_CTRL_CLR_HOST_ACK
	sh	t1,PHYS_TO_K1(IDE_RDB_CTRL_REG)	# clear interrupt
	lw	t1,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
	b	rdb_ack				# go to IP6 processing
2:
#endif
	
	and	t1,k1,t0		# and SR and CAUSE register
	and	t2,t1,CAUSE_IP7		# and with CAUSE_IP7
	beq	t2,zero,notIP7		# if CAUSE_IP7 is not set branch
	/* 
	 * Get here and rdb write interrupt has occurred, (Indy has written to U64) 
	 * See what type of data/message. If you need to send a message, load mesg  
	 * value into __osRdb_Mesg, for sending later, otherwise, go to rdbout  
	 */
	la	t1,RDB_WRITE_INTR_REG	# Clear the interrupt first.
	sw	$0, 0(t1)		# This will signal hardware
IP7check:
.set	noreorder
	mfc0	t0,C0_CAUSE		# get the cause register and put it in t0
.set	reorder
	and	t0, CAUSE_IP7
	bne	zero,t0,IP7check	# keep checking until hardware has cleared the interrupt

	la	t2,RDB_BASE_REG		# load the address of the base register
	lw	t0,0(t2)		# read the data from the base register

#ifdef BBPLAYER
rdb_req:
#endif
	srl	t1,t0,26                # Get top 6 bits (type)
	andi    t1,0x3F			# mask out other bits
#ifdef BBPLAYER
	li	t2,RDB_TYPE_HtoG_DATAB
	beq	t1,t2,HandDataB		# branch to code to handle host to game data
	/* ack and dismiss interrupt here for non DataB case */
	li	t2,IDE_RDB_CTRL_CLR_HOST_REQ|IDE_RDB_CTRL_SET_BB_ACK
	sh	t2,PHYS_TO_K1(IDE_RDB_CTRL_REG) # clear interrupt
	lw	t2,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
#endif
	li	t2,RDB_TYPE_HtoG_DATA
	beq	t1,t2,HandData		# branch to code to handle host to game data
#ifndef BBPLAYER
	li      t2,RDB_TYPE_HtoG_DEBUG  
	beq     t1,t2,HandDbg           # branch to code to handle host to game debug data
#endif
	li      t2,RDB_TYPE_HtoG_KDEBUG
	beq     t1,t2,HandKDebug
	li      t2,RDB_TYPE_HtoG_DEBUG_CT
	beq     t1,t2,DbgCnt            # branch to code to setup a host to game debug
	li	t2,RDB_TYPE_HtoG_DATA_DONE
	beq	t1,t2,DataRead          # branch to code to signal ready for next block 
	li	t2,RDB_TYPE_HtoG_LOG_DONE
	beq	t1,t2,LogRead           # completely handled the last block of log data
	li	t2,RDB_TYPE_HtoG_REQ_RAMROM
	beq	t1,t2,ReqRamrom         # host wants access to ramrom
	li	t2,RDB_TYPE_HtoG_FREE_RAMROM
	beq	t1,t2,FreeRamrom        # host doesn''t need ramrom anymore
	li      t2,RDB_TYPE_HtoG_PROF_SIGNAL
	beq     t1,t2,SignalProf

	
	b       rdbout			# don''t know what kind of data this is, branch to out

SignalProf:
	li	t2,RDB_PROF_ACK_SIG     # load the ack signal value for checking
	srl     t1,t0,16		# shift first byte of data to base of register
	andi    t1,0xFF			# and out the top bytes
	beq     t1,t2,AckProf		# if it''s an ack, branch down
	li      t2,OS_EVENT_RDB_FLUSH_PROF * ES_SIZE  # not an ack, must be a flush
	sw	t2,__osRdb_Mesg
	b	savecontext
AckProf:	
	li	t2,OS_EVENT_RDB_ACK_PROF * ES_SIZE
	sw	t2,__osRdb_Mesg
	b	savecontext
HandKDebug:	
	sw      t0, __os_Kdebug_Pkt
	b       savecontext	
DbgCnt:                                 
	li      t2, 0x00FFFFFF          # only want 24 bits
	and     t1,t0,t2                # mask out upper byte, get 3 lower bytes
	sw      t1,__osRdb_DbgRead_Ct   # store number of bytes to be read
	b	rdbout                  # don''t generate a message
		
DataRead:	
	li      t2,OS_EVENT_RDB_DATA_DONE * ES_SIZE
	sw	t2,__osRdb_Mesg         #__osRdb_DataDone
	b	savecontext             # branch to prepare for sending message
LogRead:	
	li	t2,OS_EVENT_RDB_LOG_DONE * ES_SIZE
	sw	t2,__osRdb_Mesg         # __osRdb_LogDone
	b	savecontext             # branch to prepare for sending message
ReqRamrom:	
	li      t2,OS_EVENT_RDB_REQ_RAMROM * ES_SIZE
	sw	t2,__osRdb_Mesg         # __osRdb_ReqRamrom
	b	savecontext             
FreeRamrom:	
	li	t2,OS_EVENT_RDB_FREE_RAMROM * ES_SIZE
	sw	t2,__osRdb_Mesg         # __osRdb_FreeRamrom
	b	savecontext	
HandData:
	/* 
	 * Get here, and you have received a hostio data packet
	 * Get the length, and then the right number of data bytes.
	 * If this completes the data transfer, send message, else
	 * branch to rdbout.
	 */
	srl	t1,t0,24		# get bits 24-25 (length)
	andi    t1,0x3			# get length in t1
	beq	zero,t1,rdbout		# if equal, no data!!!

	lw	t2,__osRdb_Read_Data_Ct
	subu    t2,t1			# subtract the length
	sw	t2,__osRdb_Read_Data_Ct
	
	sd	t3,T_CONTEXT_T3(k0)		# save the t3 register
	lw	t3,__osRdb_Read_Data_Buf	# load the current buffer pointer

	srl	t2,t0,16		# Get bits 16-23
	andi    t2,0xFF			# mask to get data byte
	sb	t2,0(t3)		# Store data byte
	addi    t3,1			# increment pointer
	addi	t1,-1			# decrement length
	beq	zero,t1,doneData
	srl	t2,t0,8			# get bits 8-15
	andi	t2,0xFF
	sb	t2,0(t3)
	addi	t3,1
	addi	t1,-1
	beq	zero,t1,doneData
	andi	t0,0xFF			# get bits 0-7
	sb	t0,0(t3)
	addi	t3,1

doneData:	
	sw	t3,__osRdb_Read_Data_Buf # store update value of __osRdb_Read_Data_Buf

	ld	t3,T_CONTEXT_T3(k0)	# restore t3
	lw	t2,__osRdb_Read_Data_Ct # get count of bytes still needed
	bne	zero,t2,rdbout		# more data still needed, branch to rdbout
	li	t2,OS_EVENT_RDB_READ_DONE * ES_SIZE	# get here, and need to send message
	sw	t2,__osRdb_Mesg         # __osRdb_Read_Send_Msg
	b	savecontext

#ifdef BBPLAYER
HandDataB:
	/* 
	 * Get here, and you have received a bulk hostio data packet
	 * Get the length, and then the right number of data bytes.
	 * If this completes the data transfer, send message, else
	 * branch to rdbout.
	 */
	srl	t1,t0,16		# get bits 16-23 (length)
	andi    t1,0xff			# get length in t1
#if 0
	beq	zero,t1,rdbout		# if equal, no data!!!
#endif

	lw	t2,__osRdb_Read_Data_Ct
	subu    t2,t1			# subtract the length
	sw	t2,__osRdb_Read_Data_Ct
	
	sd	t3,T_CONTEXT_T3(k0)		# save the t3 register
	lw	t3,__osRdb_Read_Data_Buf	# load the current buffer pointer
	la      t2,PHYS_TO_K1(IDE_RDB_DATA_REG+4)
	.set noat
	move	AT,t2
6:
	srl	t2,t0,8			# Get bits 8-15
	sb	t2,0(t3)		# Store data byte
	addi    t3,1			# increment pointer
	addi	t1,-1			# decrement length
	beq	zero,t1,1f
	sb	t0,0(t3)		# bits 0-7
	addi	t3,1
	addi	t1,-1
	beq	zero,t1,1f
	lhu	t0,0(AT)		# get next half-word from RDB buffer
	addiu	AT,AT,2
	b	6b
	.set at
1:
	/* ack and clear interrupt when all data has been read */
	li	t2,IDE_RDB_CTRL_CLR_HOST_REQ|IDE_RDB_CTRL_SET_BB_ACK
	sh	t2,PHYS_TO_K1(IDE_RDB_CTRL_REG) # clear interrupt
	lw	t2,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
	b	doneData
#endif
		
#ifndef BBPLAYER
HandDbg:		
	srl	t1,t0,24		# get bits 24-25 (length)
	andi    t1,0x3			# get length in t1
	beq	zero,t1,rdbout		# if equal, no data!!!

	lw	t2,__osRdb_DbgRead_Ct
	subu    t2,t1			# subtract the length
	sw	t2,__osRdb_DbgRead_Ct
	
	sd	t3,T_CONTEXT_T3(k0)	# save the t3 register
	lw	t3,__osRdb_DbgRead_Buf	# load the current buffer pointer
	bne     zero,t3,1f              # if __osRdb_DbgRead_Buf != zero, OK
	ld      t3,T_CONTEXT_T3(k0)     # else restore t3
	b       rdbout                  # and abort
1:	
	srl	t2,t0,16		# Get bits 16-23
	andi    t2,0xFF			# mask to get data byte
	sb	t2,0(t3)		# Store data byte
	addi    t3,1			# increment pointer
	addi	t1,-1			# decrement length
	beq	zero,t1,doneDbg
	srl	t2,t0,8			# get bits 8-15
	andi	t2,0xFF
	sb	t2,0(t3)
	addi	t3,1
	addi	t1,-1
	beq	zero,t1,doneDbg
	andi	t0,0xFF			# get bits 0-7
	sb	t0,0(t3)
	addi	t3,1

doneDbg:	
	sw	t3,__osRdb_DbgRead_Buf # store update value of __osRdb_DbgRead_Buf

	ld	t3,T_CONTEXT_T3(k0)	# restore t3

	lw	t2,__osRdb_DbgRead_Ct
	bne	zero,t2,rdbout		# more data still needed, branch to rdbout
	li	t2,OS_EVENT_RDB_DBG_DONE * ES_SIZE
	sw	t2,__osRdb_Mesg         # __osRdb_DbgReadDone
	b	savecontext
#endif

notIP7:
	and	t2,t1,CAUSE_IP6		# and with CAUSE_IP6
	beq	t2,zero,savecontext	# if CAUSE_IP6 is not set, branch
#ifdef BBPLAYER
	lw	t2,__osBbIsBb
	beq	t2,zero,9f
	/***XXblythe mi error, do something */
	lw	t2,PHYS_TO_K1(MI_ERROR_INFO_REG)
	b	rdbout
9:
#endif
	/* Get here and you have an rdb read interrupt, Indy has read from U64 */
	/* since we are going to eret, don''t worry about keeping cause in t0 */
	la	t1,RDB_READ_INTR_REG	# Clear the interrupt first.
	sw	$0, 0(t1)

#ifdef BBPLAYER
rdb_ack:
#endif
	lw	t2,__osRdb_IP6_Ct	# load the number of packets left to send
	bne	t2,zero,2f		# if not zero, don''t set __osRdb_IP6_Empty
	li	t2,1			
	sw	t2,__osRdb_IP6_Empty	# Set __osRdb_IP6_Empty to one (TRUE)
	b	rdbout			# branch to end of section
2:		
	addi	t2,-1			# decrement the number of packets left
	sw	t2,__osRdb_IP6_Ct	# store the new value

	lw	t0,__osRdb_IP6_Data	# get base data pointer
	lw      t1,__osRdb_IP6_CurSend	# load the offset to the current send packet
	sll     t2,t1,2			# multiply CurSend offset by 4 to get bytes
	add     t0,t2,t0		# add the base pointer and offset
	lw	t2,0(t0)		# load packet
	addi	t1,1			# increment the CurSend offset
	lw      t0,__osRdb_IP6_Size	# load Size
	sub     t0,t0,t1		# t0 = Size - CurSend
	bgtz	t0,5f			# branch if Size > CurSend
	li	t1,0			# load zero into CurSend
5:		
	sw	t1,__osRdb_IP6_CurSend	# store updated CurSend offset

#ifdef BBPLAYER
	lw	t1,__osBbIsBb
	beq	t1,zero,checkIP6	# only on a real BBPLAYER
	la	t0,PHYS_TO_K1(IDE_RDB_RDATA_REG)
	sh	t2,2(t0)		# write data to the data register
	lw	t1,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
	srl	t2,t2,16		# as 2 half-word writes
	sh	t2,0(t0)
	lw	t1,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
	li	t0,IDE_RDB_CTRL_SET_BB_REQ
	sh	t0,PHYS_TO_K1(IDE_RDB_CTRL_REG)	# generate interrupt to host
	lw	t1,PHYS_TO_K1(PI_IDE_FC_BASE_REG) # flow control
	b	rdbout
#endif

checkIP6:		
.set	noreorder
	mfc0	t0,C0_CAUSE		# get the cause register and put it in t0

.set	reorder	

	and	t0, CAUSE_IP6
	bne	zero,t0,checkIP6        # branch until hardware clears interrupt

	la	t0,RDB_BASE_REG		# load the address of the base register
	sw	t2,0(t0)		# write the data to the base register
	
rdbout:	
	/*
	 * Get here, and you aren''t going to send a message, you just want
	 * to restore the registers you have used, and return
	 */ 		
	.set	noreorder
	
	ld	t0,T_CONTEXT_T0(k0)	# restore scratch registers
	ld	t1,T_CONTEXT_T1(k0)
	ld	t2,T_CONTEXT_T2(k0)
.set noat
	ld	AT,T_CONTEXT_AT(k0)
.set at
	lw	k1,T_CONTEXT_SR(k0)
	mtc0	k1,C0_SR

	nop
	nop
	nop
	nop
	eret				# and get out of here

skip_kmc_mode:
	.set	reorder
#endif /* ifndef _FINALROM */
	
savecontext:	
	/* copy context to the current thread context */

	move	t0,k0
	lw	k0,__osRunningThread

#ifndef _FINALROM
	sw	k0,__osPreviousThread
#endif /* _FINALROM */

	ld	t1,T_CONTEXT_AT(t0)
	sd	t1,T_CONTEXT_AT(k0)
	ld	t1,T_CONTEXT_SR(t0)
	sd	t1,T_CONTEXT_SR(k0)
	ld	t1,T_CONTEXT_T0(t0)
	sd	t1,T_CONTEXT_T0(k0)
	ld	t1,T_CONTEXT_T1(t0)
	sd	t1,T_CONTEXT_T1(k0)
	ld	t1,T_CONTEXT_T2(t0)
	sd	t1,T_CONTEXT_T2(k0)
3:
	/*
	 * Save the rest of the registers.
	 */
	sd	v0,T_CONTEXT_V0(k0)
	sd	v1,T_CONTEXT_V1(k0)
	sd	a0,T_CONTEXT_A0(k0)
	sd	a1,T_CONTEXT_A1(k0)
	sd	a2,T_CONTEXT_A2(k0)
	sd	a3,T_CONTEXT_A3(k0)
	sd	t3,T_CONTEXT_T3(k0)
	sd	t4,T_CONTEXT_T4(k0)
	sd	t5,T_CONTEXT_T5(k0)
	sd	t6,T_CONTEXT_T6(k0)
	sd	t7,T_CONTEXT_T7(k0)
	sd	s0,T_CONTEXT_S0(k0)
	sd	s1,T_CONTEXT_S1(k0)
	sd	s2,T_CONTEXT_S2(k0)
	sd	s3,T_CONTEXT_S3(k0)
	sd	s4,T_CONTEXT_S4(k0)
	sd	s5,T_CONTEXT_S5(k0)
	sd	s6,T_CONTEXT_S6(k0)
	sd	s7,T_CONTEXT_S7(k0)
	sd	t8,T_CONTEXT_T8(k0)
	sd	t9,T_CONTEXT_T9(k0)
	sd	gp,T_CONTEXT_GP(k0)
	sd	sp,T_CONTEXT_SP(k0)
	sd	s8,T_CONTEXT_S8(k0)
	sd	ra,T_CONTEXT_RA(k0)
	mflo	t0
	sd	t0,T_CONTEXT_LO(k0)
	mfhi	t0
	sd	t0,T_CONTEXT_HI(k0)

	/*
	 * The __OSGlobalIntMask will be subtracted from 
	 * SR register before being stored in the thread interrupt mask.
	 */
	lw	k1,T_CONTEXT_SR(k0)
	and	t1, k1, SR_IMASK
	beqz	t1, savercp
	la	t0, __OSGlobalIntMask		# get global interrupt mask
	lw	t0, 0(t0)
	xor	t2, t0,0xffffffff
	andi	t2, SR_IMASK
	or	t4, t1, t2			# subtract __OSGlobalIntMask
	and	t3, k1,~SR_IMASK
	or	t3,t4
	sw	t3,T_CONTEXT_SR(k0)

	/* ignore the interrupts that masked off by __OSGlobalIntMask */

	andi    t0, SR_IMASK
	and     t1, t0
	and     k1,~SR_IMASK
	or      k1,t1
	
savercp:

	/*
	 * save rcp interrupt mask
	 */
	lw	t1,PHYS_TO_K1(MI_INTR_MASK_REG)	# get rcp interrupt register
	beqz	t1, endrcp
	la	t0, __OSGlobalIntMask		# get global interrupt mask
	lw	t0, 0(t0)
	srl	t0, RCP_IMASKSHIFT
	xor	t0, 0xffffffff
	and	t0, 0x3f
	lw	t4, T_CONTEXT_RCP(k0)
	and	t0, t4
	or	t1, t0				#subtract __OSGlobalIntMask
endrcp:
	sw	t1,T_CONTEXT_RCP(k0)	/*JEAN */

	.set	noreorder

	mfc0	t0,C0_EPC
	sw	t0,T_CONTEXT_PC(k0)

	/*
	 * If the thread has ever touched the floating point unit,
	 * save the floating point registers away.
	 */
	lw	t0,T_FP(k0)
	beqz	t0,1f
	nop

	cfc1	t0, $31
	nop
	sw	t0, T_CONTEXT_FPCSR(k0)
	s.d     $f0,T_CONTEXT_FP0(k0)
	s.d     $f2,T_CONTEXT_FP2(k0)
	s.d     $f4,T_CONTEXT_FP4(k0)
	s.d     $f6,T_CONTEXT_FP6(k0)
	s.d     $f8,T_CONTEXT_FP8(k0)
	s.d     $f10,T_CONTEXT_FP10(k0)
	s.d     $f12,T_CONTEXT_FP12(k0)
	s.d     $f14,T_CONTEXT_FP14(k0)
	s.d     $f16,T_CONTEXT_FP16(k0)
	s.d     $f18,T_CONTEXT_FP18(k0)
	s.d     $f20,T_CONTEXT_FP20(k0)
	s.d     $f22,T_CONTEXT_FP22(k0)
	s.d     $f24,T_CONTEXT_FP24(k0)
	s.d     $f26,T_CONTEXT_FP26(k0)
	s.d     $f28,T_CONTEXT_FP28(k0)
	s.d     $f30,T_CONTEXT_FP30(k0)

1:
	/*
	 * Get CAUSE register contents and save it away.  Although not
	 * strictly part of the thread context (it isn''t restored), save
	 * the actual bits were are using for debugging purposes.
	 *
	 * The CAUSE contents will be accessible in t0 for the remainder 
	 * of the handler.
	 */
	mfc0	t0,C0_CAUSE
	sw	t0,T_CONTEXT_CAUSE(k0)

	.set	reorder

	/*
	 * Mark the previously running thread as merely runnable.
	 */
	li	t1,OS_STATE_RUNNABLE
	sh	t1,T_STATE(k0)

#ifndef _FINALROM	
	/*
	 * If there is a message for kdebugserver, send it, else skip ahead
	 */
	lw      a0, __os_Kdebug_Pkt
	beq     a0,zero,no_kdebug
	sw      zero, __os_Kdebug_Pkt
	jal     kdebugserver 
	b       __osDispatchThreadSave
no_kdebug:		
        /*
	 * If there is an rdb message that needs to be sent, load the value
	 * into a0 and jal to send_mesg
	 */
	lw      a0, __osRdb_Mesg
	beq     a0,zero,no_rdb_mesg
 	sw      zero, __osRdb_Mesg
	jal     send_mesg

	lw      t0,T_CONTEXT_CAUSE(k0)   /* cause might have gotten corrupt by call
	                                    to send_mesg */
no_rdb_mesg:		

#endif /* ifndef _FINALROM */
		
	/*
	 * Vector to handler of interest.  Currently we are only interested
	 * in break exceptions, coprocessor unusable exceptions, and interrupts.
	 * Other interrupts go to the fault event mailbox (if someone is
	 * registered) or just cause a panic
	 */
	andi	t1,t0,CAUSE_EXCMASK		# t0 = CAUSE
	li	t2,EXC_BREAK
	beq	t1,t2,handle_break
	li	t2,EXC_CPU
	beq	t1,t2,handle_CpU
	li	t2,EXC_INT
	bne	t1,t2,panic

#ifdef BBPLAYER
	/*
	 * Check if prenmi has occurred
	 */
	la	t1,__osShutdown			# Check if we''ve seen prenmi yet
	lw	t2,0(t1)
	beqz	t2,handle_interrupt		# Skip if not shutting down
	la	t1,__osShutdownTime
	lw	t1,0(t1)
	mfc0	t2,C0_COUNT			# Compare count to shutdown time
	subu	t2,t2, t1
	li	t1,0x05000000			# 0x05000000 cycles > 1 sec
	sltu	t1,t1, t2
	beqz	t1,handle_interrupt
	jal	skExit				# if > .5 sec has elapsed, then exit
#endif

	/*
	 * We know we''re an interrupt here.  Vector to appropriate handler.
	 */
handle_interrupt:
	and	s0,k1,t0			# AND pending bits with enabled
						# k1 = SR, t0 = CAUSE
next_interrupt:
	and	t1,s0,CAUSE_IPMASK		# Mask out remaining int bits
	srl	t2,t1,CAUSE_IPSHIFT+4		# shift to high nibble
	bne	t2,zero,1f			# bits set in high nibble?
	srl	t2,t1,CAUSE_IPSHIFT		# get low nibble
	add	t2,16				# get second half of table
1:	lbu	t2,__osIntOffTable(t2)		# get value from table
	lw	t2,__osIntTable(t2)		# get handler
	j	t2				# and jump to it

	/*
	 * IP6 host has read data from rdb port, 
	 * IP7 host has writen data to the rdb port
	 * Currently, just clearing bit in s0, which does NOT clear the 
	 * interrupt or handle it. This will cause the interrupt to go 
	 * unserviced. It will be serviced when the exception handler drops 
	 * out. (it will reenter and get serviced)
	 */
IP6_Hdlr:	
	and	s0,~SR_IBIT6
	b	next_interrupt
IP7_Hdlr:
	and	s0,~SR_IBIT7
	b	next_interrupt
	
	/*
	 * Counter interrupt - clear interrupt condition and send message.
	 */
counter:
	.set	noreorder
	mfc0	t1,C0_COMPARE			# read COMPARE reg
	mtc0	t1,C0_COMPARE			# and re-write it to clear int
	.set	reorder

	li	a0,OS_EVENT_COUNTER * ES_SIZE
	jal	send_mesg
	and	s0,~SR_IBIT8
	b	next_interrupt

	/*
	 * Cartridge interrupt - INT1
	 */
cart:
	and	s0,~SR_IBIT4

	la	t1,__osHwIntTable
	addi	t1,t1,HW_INT1_OFFSET
	lw	t2,IR_HANDLER(t1)		# get handler
	beqz	t2,1f
	lw	sp,IR_STACKEND(t1)
	jal	t2
	beqz	v0, 1f
	b       redispatch

1:
#ifdef BBPLAYER
/*
 * new interrupts (flash, aes, ide, error, usb0, usb1, but, md)
 * mask the interrupt and send a message
 */
#define BB_INTR(which,label,r,v)			\
label:;							\
	andi	t1,s1,MI_INTR_##which;			\
	beqz	t1,label##x;				\
	andi	s1,~MI_INTR_##which & 0x3fc0;		\
	li	t1,v;					\
	sw	t1,PHYS_TO_K1(r);			\
	li	a0,OS_EVENT_##which * ES_SIZE;		\
	jal	send_mesg;				\
label##x:;

	lw	s1,PHYS_TO_K1(MI_EINTR_REG)	# get bcp interrupt register
	BB_INTR(FLASH,flash,PI_FLASH_CTRL_REG,0)
	BB_INTR(MD,md,MI_EINTR_REG,MI_INTR_MD)
	BB_INTR(AES,aes,MI_INTR_EMASK_REG,MI_INTR_MASK_CLR_AES)
#ifdef _FINALROM
	/* dummy handler for non-debug case */
	BB_INTR(IDE,ide,MI_INTR_EMASK_REG,MI_INTR_MASK_CLR_IDE)
#endif
	BB_INTR(PI_ERR,pi_err,MI_INTR_EMASK_REG,MI_INTR_MASK_CLR_PI_ERR)
	BB_INTR(USB0,usb0,MI_INTR_EMASK_REG,MI_INTR_MASK_CLR_USB0)
	BB_INTR(USB1,usb1,MI_INTR_EMASK_REG,MI_INTR_MASK_CLR_USB1)
	b	next_interrupt
#else
        li      a0,OS_EVENT_CART * ES_SIZE      # @@@@ KMC @@@@
	jal	send_mesg
	b	next_interrupt
#endif

	/*
	 * Rcp interrupt
	 */
rcp:
	lw	s1,PHYS_TO_K1(MI_INTR_REG)	# get rcp interrupt register
	la      t0, __OSGlobalIntMask           # get global interrupt mask
        lw      t0, 0(t0)
	srl     t0, RCP_IMASKSHIFT
	and	s1,t0				# AND pending bits with enabled
	andi	t1,s1,MI_INTR_SP
	beqz	t1,vi

	/*
	 * handle SP interrupt
	 */

	andi	s1,~MI_INTR_SP & 0x3f		# note that this was seen
	lw	t4,PHYS_TO_K1(SP_STATUS_REG)	# fetch the status register
	li	t1,(SP_CLR_INTR|SP_CLR_RSPSIGNAL)
	sw	t1,PHYS_TO_K1(SP_STATUS_REG)	# clear sp interrupt
	andi	t4, (SP_STATUS_TASKDONE | SP_STATUS_YIELDED)
	beqz	t4, sp_other_break
	li	a0,OS_EVENT_SP * ES_SIZE
	jal	send_mesg
	beqz	s1,NoMoreRcpInts		# are we done?
	b	vi				# handle the next one

sp_other_break:
	li	a0,OS_EVENT_SP_BREAK * ES_SIZE
	jal	send_mesg
	beqz	s1,NoMoreRcpInts		# are we done?

vi:
	andi	t1,s1,MI_INTR_VI
	beqz	t1,ai

	/*
	 * handle VI interrupt
	 */
	andi	s1,~MI_INTR_VI & 0x3f		# note that this was seen
	sw	zero,PHYS_TO_K1(VI_CURRENT_REG)	# clear vi interrupt
	li	a0,OS_EVENT_VI * ES_SIZE
	jal	send_mesg
	beqz	s1,NoMoreRcpInts		# are we done?

ai:
	andi	t1,s1,MI_INTR_AI
	beqz	t1,si

	/*
	 * handle AI interrupt
	 */
	andi	s1,~MI_INTR_AI & 0x3f		# note that this was seen
	li	t1,1
	sw	t1,PHYS_TO_K1(AI_STATUS_REG)	# clear ai interrupt
	li	a0,OS_EVENT_AI * ES_SIZE
	jal	send_mesg
	beqz	s1,NoMoreRcpInts		# are we done?

si:
	andi	t1,s1,MI_INTR_SI
	beqz	t1,pi

	/*
	 * handle SI interrupt
	 */
	andi	s1,~MI_INTR_SI & 0x3f		# note that this was seen
	sw	zero,PHYS_TO_K1(SI_STATUS_REG)	# clear si interrupt
	li	a0,OS_EVENT_SI * ES_SIZE
	jal	send_mesg
	beqz	s1,NoMoreRcpInts		# are we done?

pi:
	andi	t1,s1,MI_INTR_PI
	beqz	t1,dp

	/*
	 * handle PI interrupt
	 */
	andi	s1,~MI_INTR_PI & 0x3f		# note that this was seen
	li	t1,PI_CLR_INTR
	sw	t1,PHYS_TO_K1(PI_STATUS_REG)	# clear pi interrupt

	la	t1,__osPiIntTable	# load pi interrupt vecter 
	lw	t2,IR_HANDLER(t1)
	beqz	t2,1f

	lw	sp,IR_STACKEND(t1)

	move	a0,v0			# set mesg as argument to hooked-func
	jal	t2			# call hooked-function
	bnez	v0,2f			# if 0 ,nomal send message
1:	
	li	a0,OS_EVENT_PI * ES_SIZE
	jal	send_mesg
2:		
	beqz	s1,NoMoreRcpInts		# are we done?

dp:
	andi	t1,s1,MI_INTR_DP
	beqz	t1,NoMoreRcpInts
	
	/*
	 * handle DP interrupt
	 */
	andi	s1,~MI_INTR_DP & 0x3f		# note that this was seen
	li	t1,MI_CLR_DP_INTR
	sw	t1,PHYS_TO_K1(MI_BASE_REG)	# clear dp interrupt
	li	a0,OS_EVENT_DP * ES_SIZE
	jal	send_mesg

NoMoreRcpInts:

	/* 
	 * We get here when there are no more RCP interrupts to
	 * process. All threads blocked on RCP int messages have 
	 * been queued on the ready queue at this point.
	 */

	and	s0,~SR_IBIT3
	b 	next_interrupt


prenmi:
	/*
	 * Pre NMI interrupt - application need to shut down gfx and audio
	 */
	
        /*
         * We need to turn off INT2 mask bit for the old thread
         */

	lw	k1,T_CONTEXT_SR(k0)
	and	k1, ~SR_IBIT5
	sw	k1,T_CONTEXT_SR(k0)

	la	t1, __osShutdown
	lw	t2, 0(t1)
	beqz	t2, firstnmi
	and     s0,~SR_IBIT5
	b       redispatch
	
firstnmi:
	li	t2, 1
	sw	t2, 0(t1)			# Turn on shutdown mode
#ifdef BBPLAYER
	mfc0	t2,C0_COUNT			# save COUNT reg
	la	t1, __osShutdownTime
	sw	t2, 0(t1)
#endif
	li	a0,OS_EVENT_PRENMI * ES_SIZE
	jal     send_mesg
	and     s0,~SR_IBIT5
	lw	t2,__osRunQueue
	lw	k1,T_CONTEXT_SR(t2)
	and	k1, ~SR_IBIT5
	sw	k1,T_CONTEXT_SR(t2)
	b       redispatch

	/*
	 * SW2 interrupt - clear interrupt condition and send message.
	 */
sw2:
	and	t0,~CAUSE_SW2			# t0 has CAUSE reg contents
	.set	noreorder
	mtc0	t0,C0_CAUSE			# clear int by turning off bit
	.set	reorder

	li	a0,OS_EVENT_SW2 * ES_SIZE
	jal	send_mesg
	and	s0,~SR_IBIT2
	b	next_interrupt

	/*
	 * SW1 interrupt - clear interrupt condition and send message.
	 */
sw1:
	and	t0,~CAUSE_SW1			# t0 has CAUSE reg contents
	.set	noreorder
	mtc0	t0,C0_CAUSE			# clear int by turning off bit
	.set	reorder

	li	a0,OS_EVENT_SW1 * ES_SIZE
	jal	send_mesg
	and	s0,~SR_IBIT1
	b	next_interrupt

	/*
	 * Break exception - send appropriate message.
	 */
handle_break:
	li	t1,OS_FLAG_CPU_BREAK
	sh	t1,T_FLAGS(k0)
	li	a0,OS_EVENT_CPU_BREAK * ES_SIZE
	jal	send_mesg
	b	redispatch

redispatch:
	/*
	 * Now, we compare the current thread''s priority with that of the
	 * thread at the head of the run queue. If current thread''s
	 * priority is higher, we stuff it in front of the run queue and
	 * call dispatcher. If not, we simply enqueue current thread behind
	 * other threads of same priority and call dispatcher to switch
	 * to new thread.
	 */
	lw	t1,T_PRIORITY(k0)	# t1 = current.priority (k0 = current)
	lw	t2,__osRunQueue
	lw	t3,T_PRIORITY(t2)	# t3 = new.priority (t2 = new)
	bge	t1,t3,enqueueRunning	# if current.priority >= new.priority, 
					#    insert current to front of run Q
	move	a1,k0			# k0 = __osRunningThread
	la	a0,__osRunQueue		# else enqueue current normally
	jal	__osEnqueueThread	
	j	__osDispatchThread	# and jump to dispatch new thread

enqueueRunning:
	/*
	 * Here, we enqueue the current running thread to the FRONT of
	 * the run queue and call dispatcher to invoke it again. 
	 *
	 * Recall k0 = __osRunningThread
	 */

	la	t1,__osRunQueue		# t1 = list = __osRunQueue
	lw	t2,T_NEXT(t1)		# t2 = list->next
	sw	t2,T_NEXT(k0)		# new->next = list->next
	sw	k0,T_NEXT(t1)		# list->next = new
	j	__osDispatchThread

	/*
	 * Panic: store additional registers of interest and print
	 * exception frame. However, if someone (like the debugger)
	 * has registered to receive faults, just send them instead.
	 */
panic:
	sw	k0, __osFaultedThread	# store faulting thread

	li	t1,OS_STATE_STOPPED	# leave thread stopped
	sh	t1,T_STATE(k0)
	li	t1,OS_FLAG_FAULT	# set fault flag in TCB
	sh	t1,T_FLAGS(k0)
	.set	noreorder
	mfc0	t2,C0_BADVADDR		# get and save useful badvaddr
	.set	reorder
	sw	t2,T_CONTEXT_BADVADDR(k0)# k0 = __osRunningThread
	li	a0,OS_EVENT_FAULT * ES_SIZE
	jal	send_mesg
	j       __osDispatchThread	# don''t re-queue faulting thread
	END(__osException)

	/*
	 * This is now a function call, rather than a branch target.
	 * On entry, a0 contains the offset into the event queue table.
	 * If a thread has registered to receive this event, then we
	 * will re-awaken that thread if it is blocked on the queue
	 * by sending it the specified message.
	 */
LEAF(send_mesg)
	move	s2,ra			# Save return address
	la	t2,__osEventStateTab	# Need to get the message & queue
	addu	t2,a0			# a0 = index to interrupt message table

	/*
	 * Here, we prepare to send the message to the given message queue
	 */
	lw	t1,ES_QUEUE(t2)		# t2 = _InterruptMessage entry
	beqz	t1,send_done		# If empty queue, we simply return
					# t1 = message queue (mq)

	lw	t3,MQ_VALIDCOUNT(t1)	# t3 = mq->validCount
	lw	t4,MQ_MSGCOUNT(t1)	# t4 = mq->msgCount
					
	/* 
	 * If message queue is full, we simply return.
	 * if (mq->validCount >= mq->msgCount) return;
	 */
	bge	t3,t4,send_done

	/* 
	 * Calculate the next available empty slot to write message.
	 * last = (mq->first + mq->validCount) % msgCount;
	 */
	lw	t5,MQ_FIRST(t1)		# t5 = mq->first
	addu	t5,t5,t3		# t5 = first+validCount
	rem	t5,t5,t4		# t5 = (first+validCount) % msgCount

	/* 
	 * Now, we insert the message into the empty slot.
	 * mq->msg[last] = msg;
	 */
	lw	t4,MQ_MSG(t1)		# t4 = mq->msg
	mul	t5,t5,4			# t5 = msg array offset
	addu	t4,t4,t5		# t4 = message array slot
	lw	t5,ES_MESSAGE(t2)	# t5 = message
	sw	t5,0(t4)		# Write message into slot
		
	/* 
	 * Increment the valid message counter.
	 * mq->validCount++;
	 */
	addu	t2,t3,1			# t2 = validCount (t3) +1
	sw	t2,MQ_VALIDCOUNT(t1)	# Store updated validCount to msg queue

	/* 
	 * If there is no thread blocked on the queue being empty,
	 * we just return.
	 * if (mq->queue->next == NULL) return;
	 */
	lw	t2,MQ_MTQUEUE(t1)	# if mtqueue->next == NULL
	lw	t3,0(t2)		#
	beq	t3,0,send_done		# then we simply return

	/* 
	 * Otherwise, we dequeue the blocking thread and enqueue it to 
	 * the run queue behind other threads of same priority
	 */
	move	a0,t1			# else we dequeue the blocking thread,
	jal	__osPopThread
	move	t2,v0			# save returned thread in t2

	move	a1,t2			# and enqueue it to the run queue
	la	a0,__osRunQueue		# behind other threads of same priority
	jal	__osEnqueueThread

send_done:
	jr	s2			# Jump to saved return address
END(send_mesg)

	/*
	 * We got a coprocessor unusable exception, presumably due to an
	 * access to floating point.
	 *
	 * Mark the thread as a floating point thread so that we will now
	 * save its registers on context switches. Also enable the use
	 * of the floating point register so as to not fault again.
	 */
LEAF(handle_CpU)
	and	t1,t0,CAUSE_CEMASK		# t0 = CAUSE reg contents
	srl	t1,CAUSE_CESHIFT
	li	t2,1				# FPU?
	bne	t1,t2, panic			# Well, it better be

	li	t1,1
	sw	t1,T_FP(k0)			# flag thread for FP

	lw	k1,T_CONTEXT_SR(k0)		# retrieve original SR
	or	k1,SR_CU1			# make FPU usable
	sw	k1,T_CONTEXT_SR(k0)		# put it back in context

	/*
	 * We already have code to place the running thread back on
	 * the run queue and dispatch it very efficiently, so we will
	 * simply branch there. Be careful if enqueueRunning changes for
	 * any reason!! K0 does (correctly) contain __osRunningThread.
	 */
	b	enqueueRunning
END(handle_CpU)


/*
 * void __osEnqueueAndYield(OSThread **q);
 *
 * If the given queue is non-NULL, enqueue the running thread on it.
 * Then yield the processor by jumping to the dispatcher.
 *
 * All interrupts should be disabled when this routine is called.
 */
LEAF(__osEnqueueAndYield)

	.set	reorder

	/*
	 * Save context into __osRunningThread''s register area.
	 *
	 * In addition to the status register, only save the callee saved
	 * registers.  The status register value saved is that of the
	 * original contents with the SR_EXL bit set, consistent with
	 * the use of the dispatcher (see below).
	 */
	lw	a1,__osRunningThread

#ifndef _FINALROM
	sw	a1,__osPreviousThread
#endif
	
	.set	noreorder
	mfc0	t0,C0_SR
	.set	reorder

	ori	t0,SR_EXL
	sw	t0,T_CONTEXT_SR(a1)
	sd	s0,T_CONTEXT_S0(a1)
	sd	s1,T_CONTEXT_S1(a1)
	sd	s2,T_CONTEXT_S2(a1)
	sd	s3,T_CONTEXT_S3(a1)
	sd	s4,T_CONTEXT_S4(a1)
	sd	s5,T_CONTEXT_S5(a1)
	sd	s6,T_CONTEXT_S6(a1)
	sd	s7,T_CONTEXT_S7(a1)
	sd	gp,T_CONTEXT_GP(a1)
	sd	sp,T_CONTEXT_SP(a1)
	sd	s8,T_CONTEXT_S8(a1)
	sd	ra,T_CONTEXT_RA(a1)
	sw	ra,T_CONTEXT_PC(a1)

	/*
	 * If the thread has ever touched the floating point unit,
	 * save the callee saved floating point registers away.
	 */
	lw	k1,T_FP(a1)
	beqz	k1,1f

	cfc1	k1, $31
	sw	k1, T_CONTEXT_FPCSR(a1)
	s.d     $f20,T_CONTEXT_FP20(a1)
	s.d     $f22,T_CONTEXT_FP22(a1)
	s.d     $f24,T_CONTEXT_FP24(a1)
	s.d     $f26,T_CONTEXT_FP26(a1)
	s.d     $f28,T_CONTEXT_FP28(a1)
	s.d     $f30,T_CONTEXT_FP30(a1)

1:
	/*
	 * The __OSGlobalIntMask will be subtracted from 
	 * SR register before being stored in the thread interrupt mask.
	 */
	lw	k1,T_CONTEXT_SR(a1)
	and	t1, k1, SR_IMASK
	beqz	t1, 2f
	la	t0, __OSGlobalIntMask		# get global interrupt mask
	lw	t0, 0(t0)
	xor	t0, 0xffffffff
	andi	t0, SR_IMASK
	or	t1, t0				# subtract __OSGlobalIntMask
	and	k1,~SR_IMASK
	or	k1,t1
	sw	k1,T_CONTEXT_SR(a1)

2:
	/*
	 * save rcp interrupt mask
	 */
	lw	k1,PHYS_TO_K1(MI_INTR_MASK_REG)	# get rcp interrupt register
	beqz	k1, 3f
	la	k0, __OSGlobalIntMask		# get global interrupt mask
	lw	k0, 0(k0)
	srl	k0, RCP_IMASKSHIFT
	xor	k0, 0xffffffff
	and	k0, 0x3f
	lw	t0, T_CONTEXT_RCP(a1)
	and	k0, t0
	or	k1, k0				#subtract __OSGlobalIntMask
3:
	sw	k1,T_CONTEXT_RCP(a1)		/*JEAN */

	/*
	 * Put running thread onto given queue, if one is given.
	 */
	beqz	a0,noEnqueue
	jal	__osEnqueueThread
noEnqueue:
	j	__osDispatchThread
END(__osEnqueueAndYield)

/*
 * void __osEnqueueThread(OSThread **queue, OSThread *new)
 *
 * Enqueue the given thread on to the given queue after all other threads
 * of the same priority.
 *
 * NOTE: the compiled translation of this routine should not use the stack!
 * It is called directly out of the exception handler and thus it will be
 * more difficult to describe the stack requirements of the thread to the user.
 *
 *	void
 *	__osEnqueueThread(OSThread **queue, OSThread *new)
 *	{
 *		register OSThread *pred, *succ;
 *
 *		pred = (OSThread *)queue;
 *		succ = pred->next;
 *
 *		while (succ->priority >= new->priority) {
 *			pred = succ;
 *			succ = succ->next;
 *		}
 *
 *		new->next = pred->next;
 *		pred->next = new;
 *		new->queue = queue;
 *	}
 */
LEAF(__osEnqueueThread)
	move	t9,a0			# pred = (OSThread *)queue;
	lw	t8,T_NEXT(a0)		# succ = pred->next;
	lw	t7,T_PRIORITY(a1)
	lw	t6,T_PRIORITY(t8)
	blt	t6,t7,2f
1:
	move	t9,t8			# pred = succ;
	lw	t8,T_NEXT(t8)		# succ = succ->next;
	lw	t6,T_PRIORITY(t8)
	bge	t6,t7,1b
2:
	lw	t8,T_NEXT(t9)
	sw	t8,T_NEXT(a1)		# new->next = pred->next
	sw	a1,T_NEXT(t9)		# pred->next = new;
	sw	a0,T_QUEUE(a1)		# new->queue = queue;
	jr	ra
END(__osEnqueueThread)

/*
 * OSThread *__osPopThread(OSThread **queue)
 *
 * Pop (Dequeue) the highest priority thread from the given queue.
 *
 *	OSThread *
 *	__osPopThread(OSThread **queue)
 *	{
 *		register OSThread *h;
 *		register OSThread *t;
 *
 *		h = (OSThread *) queue;
 *		t = h->next;
 *		h->next = t->next;
 *	#ifdef _DEBUG
 *		t->next = (OSThread *)NULL;
 *	#endif
 *		return(t);
 *	}
 */
LEAF(__osPopThread)
	lw	v0,0(a0)		# h = (OSThread *) queue; t = h->next;
	lw	t9,T_NEXT(v0)
	sw	t9,0(a0)		# h->next = t->next;
#ifdef _DEBUG
	sw	zero,T_NEXT(v0)		# t->next = (OSThread *)NULL;
#endif
	jr	ra
END(__osPopThread)

LEAF(__osNop)
/*	sw	a0,0(sp) */

	jr	ra
END(__osNop)

		
/*
 * void __osDispatchThread(void)
 *
 * This code is the thread dispatcher. It dequeues the highest priority
 * runnable thread from the run queue, saves it as __osRunningThread,
 * restores its registers, and does an eret to restart it.
*/
LEAF(__osDispatchThread)

	la	a0,__osRunQueue
	jal	__osPopThread
	sw	v0,__osRunningThread
	li	t0, OS_STATE_RUNNING
	sh	t0, T_STATE(v0)
	move	k0,v0

	
#ifndef _FINALROM	
	/* Main logic of Thread profiler */
	/* __osThprofFunc の NULL チェック */
	la	t0,__osThprofFunc
	lw	t0,0(t0)
	beqz	t0,1f

	/* a0 にスレッド構造体の先頭アドレスを渡して callback を呼び出す */
	lw	a0,__osPreviousThread
	lw	sp,__osThprofStack
	jal	t0
1:
	
#endif /* _FINALROM */
				
__osDispatchThreadSave:


	/*
	 * We restore the SR here, but since the SR_EXL bit is on,
	 * interrupts remain disabled.
	 */
	lw	k1,T_CONTEXT_SR(k0)
	la	t0, __OSGlobalIntMask		# get global interrupt mask
	lw	t0, 0(t0)
	andi	t0, SR_IMASK
	and	t1, k1, SR_IMASK
	and	t1, t0			# merge global and thread IntMask
	and	k1,~SR_IMASK
	or	k1, t1
	.set	noreorder
	mtc0	k1,C0_SR
	.set	reorder
	.set	noat

	ld	AT,T_CONTEXT_AT(k0)
	ld	v0,T_CONTEXT_V0(k0)
	ld	v1,T_CONTEXT_V1(k0)
	ld	a0,T_CONTEXT_A0(k0)
	ld	a1,T_CONTEXT_A1(k0)
	ld	a2,T_CONTEXT_A2(k0)
	ld	a3,T_CONTEXT_A3(k0)
	ld	t0,T_CONTEXT_T0(k0)
	ld	t1,T_CONTEXT_T1(k0)
	ld	t2,T_CONTEXT_T2(k0)
	ld	t3,T_CONTEXT_T3(k0)
	ld	t4,T_CONTEXT_T4(k0)
	ld	t5,T_CONTEXT_T5(k0)
	ld	t6,T_CONTEXT_T6(k0)
	ld	t7,T_CONTEXT_T7(k0)
	ld	s0,T_CONTEXT_S0(k0)
	ld	s1,T_CONTEXT_S1(k0)
	ld	s2,T_CONTEXT_S2(k0)
	ld	s3,T_CONTEXT_S3(k0)
	ld	s4,T_CONTEXT_S4(k0)
	ld	s5,T_CONTEXT_S5(k0)
	ld	s6,T_CONTEXT_S6(k0)
	ld	s7,T_CONTEXT_S7(k0)
	ld	t8,T_CONTEXT_T8(k0)
	ld	t9,T_CONTEXT_T9(k0)
	ld	gp,T_CONTEXT_GP(k0)
	ld	sp,T_CONTEXT_SP(k0)
	ld	s8,T_CONTEXT_S8(k0)
	ld	ra,T_CONTEXT_RA(k0)
	ld	k1,T_CONTEXT_LO(k0)
	mtlo	k1
	ld	k1,T_CONTEXT_HI(k0)
	mthi	k1
	lw	k1,T_CONTEXT_PC(k0)
	.set	noreorder
	mtc0	k1,C0_EPC


	/*
	 * Restore the floating point registers only for floating point threads.
	 */

	lw	k1,T_FP(k0)
	beqz	k1,1f
	nop

	lw	k1,T_CONTEXT_FPCSR(k0)
	ctc1	k1,$31
	l.d     $f0,T_CONTEXT_FP0(k0)
	l.d     $f2,T_CONTEXT_FP2(k0)
	l.d     $f4,T_CONTEXT_FP4(k0)
	l.d     $f6,T_CONTEXT_FP6(k0)
	l.d     $f8,T_CONTEXT_FP8(k0)
	l.d     $f10,T_CONTEXT_FP10(k0)
	l.d     $f12,T_CONTEXT_FP12(k0)
	l.d     $f14,T_CONTEXT_FP14(k0)
	l.d     $f16,T_CONTEXT_FP16(k0)
	l.d     $f18,T_CONTEXT_FP18(k0)
	l.d     $f20,T_CONTEXT_FP20(k0)
	l.d     $f22,T_CONTEXT_FP22(k0)
	l.d     $f24,T_CONTEXT_FP24(k0)
	l.d     $f26,T_CONTEXT_FP26(k0)
	l.d     $f28,T_CONTEXT_FP28(k0)
	l.d     $f30,T_CONTEXT_FP30(k0)

1:
	/*
	 * restore rcp interrupt mask
	 */
	lw	k1,T_CONTEXT_RCP(k0)
	la	k0, __OSGlobalIntMask		# get global interrupt mask
	lw	k0, 0(k0)
	srl	k0, RCP_IMASKSHIFT
	and	k1, k0			# merge global and thread IntMask
	sll	k1,1
	la	k0,__osRcpImTable
	addu	k1,k0
	lhu     k1, 0(k1)
	la	k0,PHYS_TO_K1(MI_INTR_MASK_REG)
        sw      k1, 0(k0)

	nop
	nop
	nop
	nop
	eret
	.set	reorder
END(__osDispatchThread)

/*
 * void __osCleanupThread(void)
 *
 * Called when a thread returns.  Simply destroy the running thread.
 */
NESTED(__osCleanupThread, 0, ra)
	move	a0, zero
	jal	osDestroyThread
END(__osCleanupThread)
	.set mips0