*******************************************************************************
*   異常處理向量                                                              *
*   完成無法使用C的處理                                                       *
*   主要存儲必要的寄存器，以便于C的調用                                       *
*   修改：陳鈞                                                                *
*******************************************************************************

	.include "macros.ia"
	.include "board.ia"
	.extern ExcepRegInfo
	.extern ExcepStack

	EXCEP_SAVE_TEMPAREA =    ExcepRegInfo            #寄存器臨時存儲首址

	EXCEP_SP_POINTER    =    ExcepStack              #異常使用的堆棧指針 

	EXCEP_SRR0_PTR      =    0                       #寄存器SRR0的存儲地址
	EXCEP_SRR1_PTR      =    (EXCEP_SRR0_PTR + 4)    #寄存器SRR1的存儲地址
	EXCEP_DSISR_PTR     =    (EXCEP_SRR1_PTR + 4)    #寄存器DSISR的存儲地址
	EXCEP_DAR_PTR       =    (EXCEP_DSISR_PTR + 4)   #寄存器DAR的存儲地址
	EXCEP_CR_PTR        =    (EXCEP_DAR_PTR + 4)     #寄存器CR的存儲地址
	EXCEP_XER_PTR       =    (EXCEP_CR_PTR + 4)      #寄存器XER的存儲地址
	EXCEP_LR_PTR        =    (EXCEP_XER_PTR + 4)     #寄存器LR的存儲地址
	EXCEP_CTR_PTR       =    (EXCEP_LR_PTR + 4)      #寄存器CTR的存儲地址
	EXCEP_R00_PTR       =    (EXCEP_CTR_PTR + 4)     #寄存器R00的存儲地址
	EXCEP_R01_PTR       =    (EXCEP_R00_PTR + 4)     #寄存器R01的存儲地址
	EXCEP_R02_PTR       =    (EXCEP_R01_PTR + 4)     #寄存器R02的存儲地址
	EXCEP_R03_PTR       =    (EXCEP_R02_PTR + 4)     #寄存器R03的存儲地址
	EXCEP_R04_PTR       =    (EXCEP_R03_PTR + 4)     #寄存器R04的存儲地址
	EXCEP_R05_PTR       =    (EXCEP_R04_PTR + 4)     #寄存器R05的存儲地址
	EXCEP_R06_PTR       =    (EXCEP_R05_PTR + 4)     #寄存器R06的存儲地址
	EXCEP_R07_PTR       =    (EXCEP_R06_PTR + 4)     #寄存器R07的存儲地址    
	EXCEP_R08_PTR       =    (EXCEP_R07_PTR + 4)     #寄存器R08的存儲地址
	EXCEP_R09_PTR       =    (EXCEP_R08_PTR + 4)     #寄存器R09的存儲地址
	EXCEP_R10_PTR       =    (EXCEP_R09_PTR + 4)     #寄存器R10的存儲地址
	EXCEP_R11_PTR       =    (EXCEP_R10_PTR + 4)     #寄存器R11的存儲地址
	EXCEP_R12_PTR       =    (EXCEP_R11_PTR + 4)     #寄存器R12的存儲地址
	EXCEP_R13_PTR       =    (EXCEP_R12_PTR + 4)     #寄存器R13的存儲地址
	EXCEP_R14_PTR       =    (EXCEP_R13_PTR + 4)     #寄存器R14的存儲地址    
	EXCEP_R15_PTR       =    (EXCEP_R14_PTR + 4)     #寄存器R15的存儲地址
	EXCEP_R16_PTR       =    (EXCEP_R15_PTR + 4)     #寄存器R16的存儲地址
	EXCEP_R17_PTR       =    (EXCEP_R16_PTR + 4)     #寄存器R17的存儲地址
	EXCEP_R18_PTR       =    (EXCEP_R17_PTR + 4)     #寄存器R18的存儲地址
	EXCEP_R19_PTR       =    (EXCEP_R18_PTR + 4)     #寄存器R19的存儲地址
	EXCEP_R20_PTR       =    (EXCEP_R19_PTR + 4)     #寄存器R20的存儲地址
	EXCEP_R21_PTR       =    (EXCEP_R20_PTR + 4)     #寄存器R21的存儲地址
	EXCEP_R22_PTR       =    (EXCEP_R21_PTR + 4)     #寄存器R22的存儲地址
	EXCEP_R23_PTR       =    (EXCEP_R22_PTR + 4)     #寄存器R23的存儲地址
	EXCEP_R24_PTR       =    (EXCEP_R23_PTR + 4)     #寄存器R24的存儲地址
	EXCEP_R25_PTR       =    (EXCEP_R24_PTR + 4)     #寄存器R25的存儲地址
	EXCEP_R26_PTR       =    (EXCEP_R25_PTR + 4)     #寄存器R26的存儲地址
	EXCEP_R27_PTR       =    (EXCEP_R26_PTR + 4)     #寄存器R27的存儲地址
	EXCEP_R28_PTR       =    (EXCEP_R27_PTR + 4)     #寄存器R28的存儲地址    
	EXCEP_R29_PTR       =    (EXCEP_R28_PTR + 4)     #寄存器R29的存儲地址
	EXCEP_R30_PTR       =    (EXCEP_R29_PTR + 4)     #寄存器R30的存儲地址
	EXCEP_R31_PTR       =    (EXCEP_R30_PTR + 4)     #寄存器R31的存儲地址

	EXCEP_MSR_PTR       =    (EXCEP_R31_PTR + 4)     #寄存器MSR的存儲地址    
	EXCEP_VECTOR_PTR1   =    (EXCEP_MSR_PTR + 4)     #異常向量的存儲地址，判別異常種類1

	EXCEP_VECTOR_PTR2   =    (EXCEP_VECTOR_PTR1+4)   #異常的存儲地址，判異常種類2
	
	V_SAVE_AREA         =    200                     #異常信息存儲偏址
	
	
	#以下是PSOS異常處理中的一些寄存器存儲地址
	R29_OLD_PTR         =    4
	R30_OLD_PTR         =    8
	R31_OLD_PTR         =    12
	LR_OLD_PTR          =    28
	SRR0_OLD_PTR        =    36
	SRR1_OLD_PTR        =    40
	CR_OLD_PTR          =    16

	VECTOR_OLD_PTR      =    52
 
		.text
		.extern ExceptionStore       #供oss使用的函數 （用于存儲異常信息）
		.extern Ecom_ExceptionStore  
		.globl EcomExceptionHandle

		.align        2

#########################################################################
#    函數：        EcomExceptionHandle            
#    功能：        將異常發生時的寄存器內容拷貝到以EXCEP_SAVE_TEMPAREA為首址
#                的區域中
#   r31 為異常矢量地址中的存貯區的地址 vector->save_area
#########################################################################
EcomExceptionHandle:

	LA      r29,EXCEP_SAVE_TEMPAREA                 #將存儲區地址指向r29
	
	stw     r0,EXCEP_R00_PTR(r29)
	
	stw     r1,EXCEP_R01_PTR(r29)                

	stw     r2,EXCEP_R02_PTR(r29)                  #保存未被PSOS破壞的寄存器
	stw     r3,EXCEP_R03_PTR(r29)        
	stw     r4,EXCEP_R04_PTR(r29)
	stw     r5,EXCEP_R05_PTR(r29)        
	stw     r6,EXCEP_R06_PTR(r29)
	stw     r7,EXCEP_R07_PTR(r29)        
	stw     r8,EXCEP_R08_PTR(r29)
    mfcr    r9
	stw     r9,EXCEP_R09_PTR(r29)        
	stw     r10,EXCEP_R10_PTR(r29)
	stw     r11,EXCEP_R11_PTR(r29)    
	stw     r12,EXCEP_R12_PTR(r29)
	stw     r13,EXCEP_R13_PTR(r29)        
	stw     r14,EXCEP_R14_PTR(r29)
	stw     r15,EXCEP_R15_PTR(r29)        
	stw     r16,EXCEP_R16_PTR(r29)
	stw     r17,EXCEP_R17_PTR(r29)        
	stw     r18,EXCEP_R18_PTR(r29)
	stw     r19,EXCEP_R19_PTR(r29)        
	stw     r20,EXCEP_R20_PTR(r29)
	stw     r21,EXCEP_R21_PTR(r29)    
	stw     r22,EXCEP_R22_PTR(r29)
	stw     r23,EXCEP_R23_PTR(r29)        
	stw     r24,EXCEP_R24_PTR(r29)
	stw     r25,EXCEP_R25_PTR(r29)        
	stw     r26,EXCEP_R26_PTR(r29)
	stw     r27,EXCEP_R27_PTR(r29)        
	stw     r28,EXCEP_R28_PTR(r29)
	
	mfmsr   r3
	stw     r3,EXCEP_MSR_PTR(r29)
	
	lwz     r3,R29_OLD_PTR(r31)                        #保存被PSOS異常處理使用過
	stw     r3,EXCEP_R29_PTR(r29)                      #的寄存器
	
	lwz     r3,R30_OLD_PTR(r31)
	stw     r3,EXCEP_R30_PTR(r29)
	
	lwz     r3,R31_OLD_PTR(r31)
	stw     r3,EXCEP_R31_PTR(r29)    
	
	lwz     r3,LR_OLD_PTR(r31)
	stw     r3,EXCEP_LR_PTR(r29)                      #跳轉指令返回地址    
	
	lwz     r3,SRR0_OLD_PTR(r31)                      #異常發生現場的IP值
	stw     r3,EXCEP_SRR0_PTR(r29)    
	
	lwz     r3,SRR1_OLD_PTR(r31)                      #MSR的狀態
	stw     r3,EXCEP_SRR1_PTR(r29)    
	
	lwz     r3,CR_OLD_PTR(r31)
	stw     r3,EXCEP_CR_PTR(r29)                      #特殊寄存器
	
	mfspr   r3,dsisr
	stw     r3,EXCEP_DSISR_PTR(r29)                   #異常發生的種類
	
	mfspr   r3,dar
	stw     r3,EXCEP_DAR_PTR(r29)                    #DAR寄存器
	
	mfspr   r3,xer
	stw     r3,EXCEP_XER_PTR(r29)
	
	mfspr   r3,ctr
	stw     r3,EXCEP_CTR_PTR(r29)

	lwz     r3,VECTOR_OLD_PTR(r31)
	andi.    r3,r3,0x0FFFF
	stw     r3,EXCEP_VECTOR_PTR1(r29)
		   
	subi    r3,r31,V_SAVE_AREA
	stw     r3,EXCEP_VECTOR_PTR2(r29)

	LA      r5, 0x80000000
    lhz     r4,0x178(r5)         #  MSTAT Register Value
    
    andi.   r3,r4,0x0C00         #Check Bank 4,5      
	cmpwi   r3,0
	beq     other

    mr      r28,r3                     
	stw     r4,0x178(r5)
	bl      Ecom_ExceptionStore         #將異常記錄存儲    
	b       next 
		 
other:                                     
	LA      r3,ExceptionStore
	lwz     r3,0(r3)
	cmpwi   r3,0
	beq     next
	mtlr    r3
	mr      r3,r29                     #保存存儲區首址作為OSS調用的輸入參數

    bclrl   4,0                        #將異常記錄存儲    

next:	
	LA      r29,EXCEP_SAVE_TEMPAREA                 #將存儲區地址指向r29

	lwz     r0,EXCEP_R00_PTR(r29) 
	lwz     r2,EXCEP_R02_PTR(r29)                   #恢復被PSOS破壞的寄存器
	lwz     r3,EXCEP_R03_PTR(r29)        
	lwz     r4,EXCEP_R04_PTR(r29)
	lwz     r5,EXCEP_R05_PTR(r29)        
	lwz     r6,EXCEP_R06_PTR(r29)
	lwz     r7,EXCEP_R07_PTR(r29)        
	lwz     r8,EXCEP_R08_PTR(r29)
	lwz     r9,EXCEP_R09_PTR(r29)        
	mtcr	r9		                             # restore CR

	lwz     r10,EXCEP_R10_PTR(r29)
	lwz     r11,EXCEP_R11_PTR(r29)    
	lwz     r12,EXCEP_R12_PTR(r29)
	lwz     r13,EXCEP_R13_PTR(r29)        
	lwz     r14,EXCEP_R14_PTR(r29)
	lwz     r15,EXCEP_R15_PTR(r29)        
	lwz     r16,EXCEP_R16_PTR(r29)
	lwz     r17,EXCEP_R17_PTR(r29)        
	lwz     r18,EXCEP_R18_PTR(r29)
	lwz     r19,EXCEP_R19_PTR(r29)        
	lwz     r20,EXCEP_R20_PTR(r29)
	lwz     r21,EXCEP_R21_PTR(r29)    
	lwz     r22,EXCEP_R22_PTR(r29)
	lwz     r23,EXCEP_R23_PTR(r29)        
	lwz     r24,EXCEP_R24_PTR(r29)
	lwz     r25,EXCEP_R25_PTR(r29)        
	lwz     r26,EXCEP_R26_PTR(r29)
	lwz     r27,EXCEP_R27_PTR(r29)        
    mr      r3,r28
	lwz     r28,EXCEP_R28_PTR(r29)

    cmpwi   r3,0
    beq     Last

	mr      r4,r31                                 #r31->r4指向異常存儲的首地址

	lwz     r3,R29_OLD_PTR(r4)                        
	mr      r29,r3                                 #恢復被PSOS異常處理使用過的寄存器

	lwz     r3,R30_OLD_PTR(r4)
	mr      r30,r3
	
	lwz     r3,R31_OLD_PTR(r4)
	mr      r31,r3   
	
	lwz     r3,SRR1_OLD_PTR(r4)                   #MSR的狀態
	mtsrr1  r3    
	
	lwz     r3,SRR0_OLD_PTR(r4)                  #異常發生現場的IP值
	mtsrr0  r3    

    addic   r4,r3,0x04
    mtlr    r4
    
	sync                                       #
	isync                                      #
	blr
Last:
	sync                                       #
	isync                                      #
	rfi