/* entry.S - exception handler for emulating MIPS16 'entry' and 'exit'
   pseudo-instructions.  These instructions are generated by the compiler
   when the -mentry switch is used.  The instructions are not implemented
   in the MIPS16 CPU; hence the exception handler that emulates them.

   This module contains the following public functions:

   * void __install_entry_handler(void);

     This function installs the entry/exit exception handler.  It should
     be called before executing any MIPS16 functions that were compiled with
     -mentry, typically before main() is called.

   * void __remove_entry_handler(void);

     This function removes the entry/exit exception handler.  It should
     be called when the program is exiting, or when it is known that no
     more MIPS16 functions compiled with -mentry will be called.
*/

#ifdef __mips16
/* This file contains 32 bit assembly code.  */
	.set nomips16
#endif

#include "regs.S"

#define CAUSE_EXCMASK	0x3c	/* mask for ExcCode in Cause Register */
#define EXC_RI  	0x28	/* 101000 == 10 << 2 */

/* Set DEBUG to 1 to enable recording of the last 16 interrupt causes.  */

#define DEBUG 0

#if DEBUG

	.sdata
int_count:
	.space	4			/* interrupt count modulo 16 */
int_cause:
	.space	4*16			/* last 16 interrupt causes */
#endif

	.text

	.set	noreorder		/* Do NOT reorder instructions */


/* __entry_exit_handler - the reserved instruction exception handler
   that emulates the entry and exit instruction.  */

__entry_exit_handler:
	.set	noat			/* Do NOT use at register */
#if DEBUG
/* Must avoid using 'la' pseudo-op because it uses gp register, which
   may not have a good value in an exception handler. */
  
#	la	k0, int_count		/* intcount = (intcount + 1) & 0xf */
	lui	k0 ,%hi(int_count)
	addiu	k0, k0 ,%lo(int_count)
	lw	k1, (k0)
	addiu	k1, k1, 1
	andi	k1, k1, 0x0f
	sw	k1, (k0)
#	la	k0, int_cause		/* k1 = &int_cause[intcount] */
	lui	k0, %hi(int_cause)
	addiu	k0, k0, %lo(int_cause)
	sll	k1, k1, 2
	add	k1, k1, k0
#endif	
	mfc0	k0, C0_CAUSE		/* Fetch cause */
#if DEBUG
	sw	k0, -4(k1)		/* Save exception cause in buffer */
#endif
	mfc0	k1, C0_EPC		/* Check for Reserved Inst. without */
	and	k0, CAUSE_EXCMASK	/*   destroying any register */
	subu	k0, EXC_RI
	bne	k0, zero, check_others	/* Sorry, go do something else */

	and	k0, k1, 1		/* Check for TR mode (pc.0 = 1) */
	beq	k0, zero, ri_in_32	/* Sorry, RI in 32-bit mode */
	xor	k1, 1			

/* Since we now are going to emulate or die, we can use all the T-registers */
/* that MIPS16 does not use (at, t0-t8), and we don't have to save them. */

	.set	at			/* Now it's ok to use at again */

#if 0
	j	leave
	rfe
#endif

	lhu	t0, 0(k1)		/* Fetch the offending instruction */
	xor	t8, k1, 1		/* Prepare t8 for exit */
	and	t1, t0, 0xf81f		/* Check for entry/exit opcode */
	bne	t1, 0xe809, other_ri

deareg:	and	t1, t0, 0x0700		/* Isolate the three a-bits */
	srl	t1, 6			/* Adjust them so x4 is applied */
	slt     t2, t1, 17		/* See if this is the exit instruction */
	beqz    t2, doexit
	la	t2, savea
	subu	t2, t1
	jr	t2			/* Jump into the instruction table */
	rfe				/* We run the rest in user-mode */

					/* This is the entry instruction! */
	sw	a3, 12(sp)		/* 4: a0-a3 saved */
	sw	a2,  8(sp)		/* 3: a0-a2 saved */
	sw	a1,  4(sp)		/* 2: a0-a1 saved */
	sw	a0,  0(sp)		/* 1: a0    saved */
savea:					/* 0: No arg regs saved */

dera:	and	t1, t0, 0x0020		/* Isolate the save-ra bit */
	move	t7, sp			/* Temporary SP */
	beq	t1, zero, desreg
	subu	sp, 32			/* Default SP adjustment */
	sw	ra, -4(t7)
	subu	t7, 4

desreg:	and	t1, t0, 0x00c0		/* Isolate the two s-bits */
	beq	t1, zero, leave
	subu	t1, 0x0040
	beq	t1, zero, leave		/* Only one to save... */
	sw	s0, -4(t7)		/* Do the first one */
	sw	s1, -8(t7)		/* Do the last one */

leave:	jr	t8			/* Exit to unmodified EPC */
	nop				/* Urgh - the only nop!! */

doexf0: mtc1	v0,$f0			/* Copy float value */
	b       doex2

doexf1:	mtc1	v1,$f0			/* Copy double value */
	mtc1    v0,$f1
	b       doex2

doexit:	slt	t2, t1, 21
	beq	t2, zero, doexf0
	slt	t2, t1, 25
	beq	t2, zero, doexf1

doex2:	and	t1, t0, 0x0020		/* Isolate ra bit */
	beq     t1, zero, dxsreg	/* t1 holds ra-bit */
	addu	t7, sp, 32		/* Temporary SP */
	lw	ra, -4(t7)
	subu	t7, 4

dxsreg:	and	t1, t0, 0x00c0		/* Isolate the two s-bits */
	beq	t1, zero, leavex
	subu	t1, 0x0040
	beq	t1, zero, leavex	/* Only one to save... */
	lw	s0, -4(t7)		/* Do the first one */
	lw	s1, -8(t7)		/* Do the last one */

leavex:	jr	ra			/* Exit to ra */
	addu	sp, 32			/* Clean up stack pointer */

/* Come here for exceptions we can't handle.  */

ri_in_32:
other_ri:
check_others:				/* call the previous handler */
	la	k0,__previous
	jr	k0
	nop

__exception_code:
	.set noreorder
	la	k0, __entry_exit_handler
#	lui	k0, %hi(exception)
#	addiu	k0, k0, %lo(exception)
	jr	k0
	nop
	.set reorder
__exception_code_end:

	.data
__previous:
	.space	(__exception_code_end - __exception_code)
	.text


/* void __install_entry_handler(void)

   Install our entry/exit reserved instruction exception handler.
*/
	.ent	__install_entry_handler
	.globl	__install_entry_handler
__install_entry_handler:
        .set noreorder
	mfc0	a0,C0_SR
	nop
	li	a1,SR_BEV
	and	a1,a1,a0
	beq	a1,$0,baseaddr
	lui	a0,0x8000	/* delay slot */
	lui	a0,0xbfc0
	addiu	a0,a0,0x0100
baseaddr:
	addiu	a0,a0,0x080	/* a0 = base vector table address */
	li	a1,(__exception_code_end - __exception_code)
	la	a2,__exception_code
	la	a3,__previous
/* there must be a better way of doing this???? */
copyloop:
	lw	v0,0(a0)
	sw	v0,0(a3)
	lw	v0,0(a2)
	sw	v0,0(a0)
	addiu	a0,a0,4
	addiu	a2,a2,4
	addiu	a3,a3,4
	subu	a1,a1,4
	bne	a1,$0,copyloop
	nop
	j	ra
	nop
        .set reorder
	.end	__install_entry_handler


/* void __remove_entry_handler(void);

   Remove our entry/exit reserved instruction exception handler.
*/

	.ent	__remove_entry_handler
	.globl	__remove_entry_handler
__remove_entry_handler:
        .set noreorder

	mfc0	a0,C0_SR
	nop
	li	a1,SR_BEV
	and	a1,a1,a0
	beq	a1,$0,res_baseaddr
	lui	a0,0x8000	/* delay slot */
	lui	a0,0xbfc0
	addiu	a0,a0,0x0200
res_baseaddr:
	addiu	a0,a0,0x0180	/* a0 = base vector table address */
	li	a1,(__exception_code_end - __exception_code)
	la	a3,__previous

/* there must be a better way of doing this???? */
res_copyloop:
	lw	v0,0(a3)
	sw	v0,0(a0)
	addiu	a0,a0,4
	addiu	a3,a3,4
	subu	a1,a1,4
	bne	a1,$0,res_copyloop
	nop
	j	ra
	nop
        .set reorder
	.end	__remove_entry_handler


/* software_init_hook - install entry/exit handler and arrange to have it
   removed at exit.  This function is called by crt0.S.  */

	.text
	.globl	software_init_hook
	.ent	software_init_hook
software_init_hook:
	.set	noreorder
	subu	sp, sp, 8			/* allocate stack space */
	sw	ra, 4(sp)			/* save return address */
	jal	__install_entry_handler		/* install entry/exit handler */
	nop
	lui	a0, %hi(__remove_entry_handler)	/* arrange for exit to */
	jal	atexit				/*  de-install handler */
	addiu	a0, a0, %lo(__remove_entry_handler)	/* delay slot */
	lw	ra, 4(sp)			/* get return address */
	j	ra				/* return */
	addu	sp, sp, 8			/* deallocate stack */
	.set	reorder
	.end	software_init_hook