excep_s.s

MIPS下的boottloader yamon 的源代码

        /* Needed for all types of exceptions */
	MFC0(   t0, C0_CAUSE )

	/* Determine the cause */
	li	t1, CAUSE_EXC
	beq	k0, t1, exc_normal
	move	a1, zero    /* Parameter for registered_exc_handler() */

	/* EJTAG exception */
	li	a1, 1       /* Parameter for registered_exc_handler() */

exc_normal :
	/**** Exception ****/	

	/* a0 is set to the cause code field of the CAUSE register */
	li      t1, C0_CAUSE_CODE_MSK
	and     a0, t0, t1
	srl     a0, C0_CAUSE_CODE_SHF

	/* Jump to registered exception handler */
	la      k1, registered_exc_handler
	lw	k1, 0(k1)
	jalr	k1
	nop

	/* If exception handler returns, we jump to restore code */
	la	a0,  EXCEP_gdb_regs
	b	EXCEP_exc_handler_ret
	nop
		
END( EXCEP_install_exc_in_ram )	


	
LEAF( EXCEP_exc_handler_ret )
	
	/*  Return context and return from exception.
	 *
	 *  a0 = Pointer to context to be restored.
	 */

	/* Restore cp0 registers */
	jal	restore_cp0_regs
	move	k0, a0
		
	la	k1, restore_done
	b	restore_cpu_regs
	nop

restore_done:

	/* Determine eret/deret based on cause */
	la	k0, cause
	lw	k0, 0(k0)
	li	k1, CAUSE_EJTAG
	beq	k0, k1, return_from_ejtag
	nop

	/* Normal return from exception */
	.set mips3
	eret
	.set mips0

return_from_ejtag:

	/*  Return from EJTAG exception
	 *  deret will not clear EXL, so we do it.
	 */
	MFC0( k0, C0_STATUS )
	li    k1, ~C0_STATUS_EXL_BIT
	and   k0, k1
	MTC0( k0, C0_STATUS )

	DERET
	nop
	
END( EXCEP_exc_handler_ret )	


	
LEAF( EXCEP_exc_handler_jump )

	/*  Restore context and jump to function given by a0.
	 *  The function is responsible for handling the
	 *  exception and possibly issue an eret (or deret
	 *  in the case of an EJTAG exception).
	 *
	 *  Function is of type t_EXCEP_esr defined as :	
	 *    typedef void (*t_EXCEP_esr)(void);
	 *
	 *  a0 = Pointer to function taking over (assume 32 bit)
	 */

	/* Set context to be restored */
	la	k0, EXCEP_gdb_regs			

	/* Restore cp0 registers */	
	jal	restore_cp0_regs
	nop

	/*  a0 holds return address. Move this to k1, since
	 *  restore_cpu_regs() will perform a jr k1
	 */
	move	k1, a0
	b	restore_cpu_regs
	nop

END( EXCEP_exc_handler_jump )	


SLEAF( restore_cpu_regs )

        /* k0 points to context, k1 = return address */
        
	la	t0,   sys_64bit
	lb	t0,   0(t0)
	bne	t0,   zero, restore_64bit
	nop

	/* Restore 32 bit CPU register bank */	
	lw	$0,   (GDB_FR_REG0  + OFS_END)(k0)
	.set noat
	lw	$1,   (GDB_FR_REG1  + OFS_END)(k0)
	.set at
	lw	$2,   (GDB_FR_REG2  + OFS_END)(k0)
	lw	$3,   (GDB_FR_REG3  + OFS_END)(k0)
	lw	$4,   (GDB_FR_REG4  + OFS_END)(k0)
	lw	$5,   (GDB_FR_REG5  + OFS_END)(k0)
	lw	$6,   (GDB_FR_REG6  + OFS_END)(k0)
	lw	$7,   (GDB_FR_REG7  + OFS_END)(k0)
	lw	$8,   (GDB_FR_REG8  + OFS_END)(k0)
	lw	$9,   (GDB_FR_REG9  + OFS_END)(k0)
	lw	$10,  (GDB_FR_REG10 + OFS_END)(k0)
	lw	$11,  (GDB_FR_REG11 + OFS_END)(k0)
	lw	$12,  (GDB_FR_REG12 + OFS_END)(k0)
	lw	$13,  (GDB_FR_REG13 + OFS_END)(k0)
	lw	$14,  (GDB_FR_REG14 + OFS_END)(k0)
	lw	$15,  (GDB_FR_REG15 + OFS_END)(k0)
	lw	$16,  (GDB_FR_REG16 + OFS_END)(k0)
	lw	$17,  (GDB_FR_REG17 + OFS_END)(k0)
	lw	$18,  (GDB_FR_REG18 + OFS_END)(k0)
	lw	$19,  (GDB_FR_REG19 + OFS_END)(k0)
	lw	$20,  (GDB_FR_REG20 + OFS_END)(k0)
	lw	$21,  (GDB_FR_REG21 + OFS_END)(k0)
	lw	$22,  (GDB_FR_REG22 + OFS_END)(k0)
	lw	$23,  (GDB_FR_REG23 + OFS_END)(k0)
	lw	$24,  (GDB_FR_REG24 + OFS_END)(k0)
	lw	$25,  (GDB_FR_REG25 + OFS_END)(k0)
	/* Not k0 and k1 = $26, $27 */	
	lw	$28,  (GDB_FR_REG28 + OFS_END)(k0)
	lw	$29,  (GDB_FR_REG29 + OFS_END)(k0)
	lw	$30,  (GDB_FR_REG30 + OFS_END)(k0)
	lw	$31,  (GDB_FR_REG31 + OFS_END)(k0)

	jr	k1
	nop

restore_64bit :
	/* Restore 64 bit CPU register bank */	

	OPCODE_LD      (0,  GDB_FR_REG0,  SYS_CPUREG_K0 )
	OPCODE_LD      (1,  GDB_FR_REG1,  SYS_CPUREG_K0 )
	OPCODE_LD      (2,  GDB_FR_REG2,  SYS_CPUREG_K0 )
	OPCODE_LD      (3,  GDB_FR_REG3,  SYS_CPUREG_K0 )
	OPCODE_LD      (4,  GDB_FR_REG4,  SYS_CPUREG_K0 )
	OPCODE_LD      (5,  GDB_FR_REG5,  SYS_CPUREG_K0 )
	OPCODE_LD      (6,  GDB_FR_REG6,  SYS_CPUREG_K0 )
	OPCODE_LD      (7,  GDB_FR_REG7,  SYS_CPUREG_K0 )
	OPCODE_LD      (8,  GDB_FR_REG8,  SYS_CPUREG_K0 )
	OPCODE_LD      (9,  GDB_FR_REG9,  SYS_CPUREG_K0 )
	OPCODE_LD      (10, GDB_FR_REG10, SYS_CPUREG_K0 )
	OPCODE_LD      (11, GDB_FR_REG11, SYS_CPUREG_K0 )
	OPCODE_LD      (12, GDB_FR_REG12, SYS_CPUREG_K0 )
	OPCODE_LD      (13, GDB_FR_REG13, SYS_CPUREG_K0 )
	OPCODE_LD      (14, GDB_FR_REG14, SYS_CPUREG_K0 )
	OPCODE_LD      (15, GDB_FR_REG15, SYS_CPUREG_K0 )
	OPCODE_LD      (16, GDB_FR_REG16, SYS_CPUREG_K0 )
	OPCODE_LD      (17, GDB_FR_REG17, SYS_CPUREG_K0 )
	OPCODE_LD      (18, GDB_FR_REG18, SYS_CPUREG_K0 )
	OPCODE_LD      (19, GDB_FR_REG19, SYS_CPUREG_K0 )
	OPCODE_LD      (20, GDB_FR_REG20, SYS_CPUREG_K0 )
	OPCODE_LD      (21, GDB_FR_REG21, SYS_CPUREG_K0 )
	OPCODE_LD      (22, GDB_FR_REG22, SYS_CPUREG_K0 )
	OPCODE_LD      (23, GDB_FR_REG23, SYS_CPUREG_K0 )
	OPCODE_LD      (24, GDB_FR_REG24, SYS_CPUREG_K0 )
	OPCODE_LD      (25, GDB_FR_REG25, SYS_CPUREG_K0 )
	/* Not k0, k1 = $26, $27 */
	OPCODE_LD      (28, GDB_FR_REG28, SYS_CPUREG_K0 )
	OPCODE_LD      (29, GDB_FR_REG29, SYS_CPUREG_K0 )
	OPCODE_LD      (30, GDB_FR_REG30, SYS_CPUREG_K0 )
	OPCODE_LD      (31, GDB_FR_REG31, SYS_CPUREG_K0 )

	jr	k1
	nop

END( restore_cpu_regs )


LEAF( sys_store_cp0_regs )

	la	t0,   sys_64bit
	lb	t0,   0(t0)
	bne	t0,   zero, store_64bit_cp0
	nop
		
	MFC0(   a1, C0_STATUS)
	sw	a1, (GDB_FR_STATUS       + OFS_END)(a0)
	sw	a1, (GDB_FR_CP0_REG12    + OFS_END)(a0)

	MFC0(   a1, C0_BADVADDR )
	sw	a1, (GDB_FR_BADVADDR     + OFS_END)(a0)
	sw	a1, (GDB_FR_CP0_REG8     + OFS_END)(a0)	
	
	MFC0(   a1, C0_CAUSE )
	sw	a1, (GDB_FR_CAUSE        + OFS_END)(a0)
	sw	a1, (GDB_FR_CP0_REG13    + OFS_END)(a0)

	MFC0(   a1, C0_EPC )
	sw	a1, (GDB_FR_EPC          + OFS_END)(a0)
	sw	a1, (GDB_FR_CP0_REG14    + OFS_END)(a0)
	
	mflo	a1
	sw	a1, (GDB_FR_LO           + OFS_END)(a0)

	mfhi	a1
	sw	a1, (GDB_FR_HI           + OFS_END)(a0)		

	MFC0(   a1, C0_INDEX )
	sw	a1, (GDB_FR_CP0_INDEX    + OFS_END)(a0)

	MFC0(   a1, C0_RANDOM )
	sw	a1, (GDB_FR_CP0_RANDOM   + OFS_END)(a0)

	MFC0(   a1, C0_ENTRYLO0 )
	sw	a1, (GDB_FR_CP0_ENTRYLO0 + OFS_END)(a0)
		
	MFC0(   a1, C0_ENTRYLO1 )
	sw	a1, (GDB_FR_CP0_ENTRYLO1 + OFS_END)(a0)

	MFC0(   a1, C0_CONTEXT )
	sw	a1, (GDB_FR_CP0_CONTEXT  + OFS_END)(a0)

	MFC0(   a1, C0_PAGEMASK )
	sw	a1, (GDB_FR_CP0_PAGEMASK + OFS_END)(a0)

	MFC0(   a1, C0_WIRED )
	sw	a1, (GDB_FR_CP0_WIRED    + OFS_END)(a0)

	MFC0(   a1, C0_COUNT )
	sw	a1, (GDB_FR_CP0_REG9     + OFS_END)(a0)

	MFC0(   a1, C0_ENTRYHI )
	sw	a1, (GDB_FR_CP0_ENTRYHI  + OFS_END)(a0)

	MFC0(   a1, C0_COMPARE )
	sw	a1, (GDB_FR_CP0_REG11    + OFS_END)(a0)

	MFC0(   a1, C0_PRID )
	sw	a1, (GDB_FR_CP0_PRID     + OFS_END)(a0)

	MFC0(   a1, C0_ERROREPC )
	sw	a1, (GDB_FR_CP0_ERROREPC + OFS_END)(a0)

	MFC0(   a1, C0_CONFIG )
	sw	a1, (GDB_FR_CP0_CONFIG   + OFS_END)(a0)

	MFC0(   a1, C0_WATCHLO )
	sw	a1, (GDB_FR_CP0_WATCHLO  + OFS_END)(a0)

	MFC0(   a1, C0_WATCHHI )
	sw	a1, (GDB_FR_CP0_WATCHHI  + OFS_END)(a0)

	MFC0(   a1, C0_DEBUG )
	sw	a1, (GDB_FR_CP0_DEBUG    + OFS_END)(a0)

	MFC0(   a1, C0_DEPC )
	sw	a1, (GDB_FR_CP0_DEPC     + OFS_END)(a0)
	
	jr ra
	nop

store_64bit_cp0 :	

	MFC0(      a1, C0_STATUS)
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_STATUS,       SYS_CPUREG_A0 )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_REG12,    SYS_CPUREG_A0 )

	MFC0(   a1, C0_BADVADDR )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_BADVADDR,     SYS_CPUREG_A0 )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_REG8,     SYS_CPUREG_A0 )
	
	MFC0(   a1, C0_CAUSE )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CAUSE,        SYS_CPUREG_A0 )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_REG13,    SYS_CPUREG_A0 )	

	MFC0(   a1, C0_EPC )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_EPC,          SYS_CPUREG_A0 )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_REG14,    SYS_CPUREG_A0 )
	
	mflo	a1
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_LO,           SYS_CPUREG_A0 )

	mfhi	a1
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_HI,           SYS_CPUREG_A0 )

	MFC0(   a1, C0_INDEX )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_INDEX,    SYS_CPUREG_A0 )

	MFC0(   a1, C0_RANDOM )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_RANDOM,   SYS_CPUREG_A0 )
	
	MFC0(   a1, C0_ENTRYLO0 )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_ENTRYLO0, SYS_CPUREG_A0 )
		
	MFC0(   a1, C0_ENTRYLO1 )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_ENTRYLO1, SYS_CPUREG_A0 )

	MFC0(   a1, C0_CONTEXT )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_CONTEXT,  SYS_CPUREG_A0 )

	MFC0(   a1, C0_PAGEMASK )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_PAGEMASK, SYS_CPUREG_A0 )

	MFC0(   a1, C0_WIRED )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_WIRED,    SYS_CPUREG_A0 )

	MFC0(   a1, C0_COUNT )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_REG9,     SYS_CPUREG_A0 )
	
	MFC0(   a1, C0_ENTRYHI )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_ENTRYHI,  SYS_CPUREG_A0 )

	MFC0(   a1, C0_COMPARE )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_REG11,    SYS_CPUREG_A0 )

	MFC0(   a1, C0_PRID )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_PRID,     SYS_CPUREG_A0 )

	MFC0(   a1, C0_ERROREPC )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_ERROREPC, SYS_CPUREG_A0 )

	MFC0(   a1, C0_CONFIG )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_CONFIG,   SYS_CPUREG_A0 )

	MFC0(   a1, C0_WATCHLO )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_WATCHLO,  SYS_CPUREG_A0 )

	MFC0(   a1, C0_WATCHHI )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_WATCHHI,  SYS_CPUREG_A0 )

	MFC0(   a1, C0_DEBUG )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_DEBUG,    SYS_CPUREG_A0 )

	MFC0(   a1, C0_DEPC )
	OPCODE_SD( SYS_CPUREG_A1, GDB_FR_CP0_DEPC,     SYS_CPUREG_A0 )
	
	jr ra
	nop
	
END( sys_store_cp0_regs )


/************************************************************************
 *  Implementation : Static functions
 ************************************************************************/

	
SLEAF( restore_cp0_regs )

	la	t0, sys_64bit
	lb	t0, 0(t0)
	bne	t0, zero, restore_64bit_cp0
	nop
	
	lw	k1, (GDB_FR_CAUSE        + OFS_END)(k0)
	MTC0(   k1, C0_CAUSE )

	lw	k1, (GDB_FR_EPC          + OFS_END)(k0)
	MTC0(   k1, C0_EPC )
	
	lw	k1, (GDB_FR_LO           + OFS_END)(k0)
	mtlo	k1

	lw	k1, (GDB_FR_HI           + OFS_END)(k0)		
	mthi	k1

	lw	k1, (GDB_FR_CP0_INDEX    + OFS_END)(k0)
	MTC0(   k1, C0_INDEX )

	lw	k1, (GDB_FR_CP0_ENTRYLO0 + OFS_END)(k0)
	MTC0(   k1, C0_ENTRYLO0 )

	lw	k1, (GDB_FR_CP0_ENTRYLO1 + OFS_END)(k0)
	MTC0(   k1, C0_ENTRYLO1 )

	lw	k1, (GDB_FR_CP0_CONTEXT  + OFS_END)(k0)
	MTC0(   k1, C0_CONTEXT )

	lw	k1, (GDB_FR_CP0_PAGEMASK + OFS_END)(k0)
	MTC0(   k1, C0_PAGEMASK )

	lw	k1, (GDB_FR_CP0_WIRED    + OFS_END)(k0)
	MTC0(   k1, C0_WIRED )

#if 0
	lw	k1, (GDB_FR_CP0_REG9     + OFS_END)(k0)
	MTC0(   k1, C0_COUNT )

	lw	k1, (GDB_FR_CP0_REG11    + OFS_END)(k0)
	MTC0(   k1, C0_COMPARE )
#endif
	
	lw	k1, (GDB_FR_CP0_ENTRYHI  + OFS_END)(k0)
	MTC0(   k1, C0_ENTRYHI )

	lw	k1, (GDB_FR_CP0_ERROREPC + OFS_END)(k0)
	MTC0(   k1, C0_ERROREPC )

	lw	k1, (GDB_FR_CP0_CONFIG   + OFS_END)(k0)
	MTC0(   k1, C0_CONFIG )

	lw	k1, (GDB_FR_CP0_WATCHLO  + OFS_END)(k0)
	MTC0(   k1, C0_WATCHLO )

	lw	k1, (GDB_FR_CP0_WATCHHI  + OFS_END)(k0)
	MTC0(   k1, C0_WATCHHI )

	lw	k1, (GDB_FR_STATUS       + OFS_END)(k0)
	MTC0(	k1, C0_STATUS )

	/* TBD : consider DEBUG */
#if 0
	lw	k1, (GDB_FR_CP0_DEBUG    + OFS_END)(k0)	
	MTC0(   k1, C0_DEBUG )
#endif

	lw	k1, (GDB_FR_CP0_DEPC     + OFS_END)(k0)
	MTC0(   k1, C0_DEPC )

	jr ra
	nop

restore_64bit_cp0 :

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CAUSE,        SYS_CPUREG_K0 )
	MTC0(   k1, C0_CAUSE )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_EPC,          SYS_CPUREG_K0 )
	MTC0(   k1, C0_EPC )
	
	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_LO,           SYS_CPUREG_K0 )
	mtlo	k1

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_HI,           SYS_CPUREG_K0 )
	mthi	k1

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_INDEX,    SYS_CPUREG_K0 )
	MTC0(   k1, C0_INDEX )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_ENTRYLO0, SYS_CPUREG_K0 )
	MTC0(   k1, C0_ENTRYLO0 )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_ENTRYLO1, SYS_CPUREG_K0 )
	MTC0(   k1, C0_ENTRYLO1 )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_CONTEXT,  SYS_CPUREG_K0 )
	MTC0(   k1, C0_CONTEXT )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_PAGEMASK, SYS_CPUREG_K0 )
	MTC0(   k1, C0_PAGEMASK )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_WIRED,    SYS_CPUREG_K0 )
	MTC0(   k1, C0_WIRED )

#if 0
	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_REG9,     SYS_CPUREG_K0 )
	MTC0(   k1, C0_COUNT )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_REG11,    SYS_CPUREG_K0 )
	MTC0(   k1, C0_COMPARE )
#endif
	
	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_ENTRYHI,  SYS_CPUREG_K0 )
	MTC0(   k1, C0_ENTRYHI )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_ERROREPC, SYS_CPUREG_K0 )
	MTC0(   k1, C0_ERROREPC )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_CONFIG,   SYS_CPUREG_K0 )
	MTC0(   k1, C0_CONFIG )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_WATCHLO,  SYS_CPUREG_K0 )
	MTC0(   k1, C0_WATCHLO )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_WATCHHI,  SYS_CPUREG_K0 )
	MTC0(   k1, C0_WATCHHI )

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_STATUS,   SYS_CPUREG_K0 )
	MTC0(	k1, C0_STATUS )

	/* TBD : consider DEBUG */
#if 0
	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_DEBUG,    SYS_CPUREG_K0 )
	MTC0(   k1, C0_DEBUG )
#endif

	OPCODE_LD( SYS_CPUREG_K1, GDB_FR_CP0_DEPC,     SYS_CPUREG_K0 )
	MTC0(   k1, C0_DEPC )

	jr ra
	nop
	
END( restore_cp0_regs )