Subversion Repositories or1k

/* 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