/* Emulation of M68020 and M68881 instructions */

#include "params.h"
#include "gambit.h"
#include "struct.h"
#include "os.h"
#include "opcodes.h"


/*---------------------------------------------------------------------------*/

char *emul_code_alloc, *emul_code_top;


#define MAX_GEN_WORDS 32

short gen[MAX_GEN_WORDS];
short *gen_ptr;
long code_len;

void gen_ea( ea, code_ptr )
long ea;
short *code_ptr;
{ if (((ea&0x38)==0x28) || (ea==0x3c) || (ea==0x3a))
    *gen_ptr++ = code_ptr[code_len++];
  if (ea==0x3c) *gen_ptr++ = code_ptr[code_len++];
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

void long_mul()
{ pstate->temp[1] = pstate->temp[1] * pstate->temp[0];
}


void long_div()
{ pstate->temp[1] = pstate->temp[2] % pstate->temp[0];
  pstate->temp[2] = pstate->temp[2] / pstate->temp[0];
}


long emul_M68020_instr( code_ptr )
short *code_ptr;
{ short *p1, *p2, *p3;
  gen_ptr = gen;
  code_len = 2;
  *gen_ptr++ = MOVE_L_PINCA7_A5_DISP_OP;
  *gen_ptr++ = pstate_offset( &pstate->temp[3] );
  switch (code_ptr[0] & 0xffc0)
  { case 0x4c00: /* MULS.L <ea>,Dl */
    { long ea, dl_reg;
      if ((code_ptr[1] & 0x8ff8) != 0x0800) goto error;
      dl_reg = (code_ptr[1] & 0x7000) >> 12;
      ea = code_ptr[0] & 0x3f;
      *gen_ptr++ = MOVE_L_EA_A5_DISP_OP+ea;
      gen_ea( ea, code_ptr );
      *gen_ptr++ = pstate_offset( &pstate->temp[0] );
      *gen_ptr++ = MOVE_L_EA_A5_DISP_OP+dl_reg;
      *gen_ptr++ = pstate_offset( &pstate->temp[1] );
      *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
      *(void (**)())gen_ptr = long_mul;  gen_ptr += 2;
      *gen_ptr++ = JSRA6_DISP_OP;
      *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
      *gen_ptr++ = MOVE_L_A5_DISP_EA_OP+(dl_reg<<9);
      *gen_ptr++ = pstate_offset( &pstate->temp[1] );
      break;
    }
    case 0x4c40: /* DIVS.L <ea>,Dr:Dq */
    { long ea, dr_reg, dq_reg;
      if ((code_ptr[1] & 0x8ff8) != 0x0800) goto error;
      dr_reg = code_ptr[1] & 0x7;
      dq_reg = (code_ptr[1] & 0x7000) >> 12;
      ea = code_ptr[0] & 0x3f;
      *gen_ptr++ = MOVE_L_EA_A5_DISP_OP+ea;
      gen_ea( ea, code_ptr );
      *gen_ptr++ = pstate_offset( &pstate->temp[0] );
      *gen_ptr++ = MOVE_L_EA_A5_DISP_OP+dr_reg;
      *gen_ptr++ = pstate_offset( &pstate->temp[1] );
      *gen_ptr++ = MOVE_L_EA_A5_DISP_OP+dq_reg;
      *gen_ptr++ = pstate_offset( &pstate->temp[2] );
      *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
      *(void (**)())gen_ptr = long_div;  gen_ptr += 2;
      *gen_ptr++ = JSRA6_DISP_OP;
      *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
      *gen_ptr++ = MOVE_L_A5_DISP_EA_OP+(dr_reg<<9);
      *gen_ptr++ = pstate_offset( &pstate->temp[1] );
      *gen_ptr++ = MOVE_L_A5_DISP_EA_OP+(dq_reg<<9);
      *gen_ptr++ = pstate_offset( &pstate->temp[2] );
      break;
    }
    default:
      goto error;
  }
  *gen_ptr++ = MOVE_L_A5_DISP_PDECA7_OP;
  *gen_ptr++ = pstate_offset( &pstate->temp[3] );
  *gen_ptr++ = RTS_OP;
  p1 = (short *)pstate->emul_code_bot;
  while (p1 < (short *)emul_code_alloc)
  { p2 = p1;
    p3 = gen;
    while ((p3 < gen_ptr) && (*p2 == *p3)) { p2++; p3++; }
    if (p3 == gen_ptr)
    { *code_ptr++ = JSRA5_DISP_OP;
      *code_ptr++ = pstate_offset( p1 );
      while (code_len > 2) { *code_ptr++ = NOP_OP; code_len--; }
      return 0;
    }
    p1++;
  }
  if (emul_code_alloc+(gen_ptr-gen)*sizeof(short) > emul_code_top)
  { os_err = "Emulation code memory overflow"; return 1; }
  *code_ptr++ = JSRA5_DISP_OP;
  *code_ptr++ = pstate_offset( emul_code_alloc );
  while (code_len > 2) { *code_ptr++ = NOP_OP; code_len--; }
  p1 = gen;
  while (p1 < gen_ptr)
  { *(short *)emul_code_alloc = *p1++;
    emul_code_alloc += sizeof(short);
  }
  return 0;
  error:
  os_err = "Unknown M68020 instruction";
  return 1;
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

#ifdef NO_EMUL_M68881

long emul_M68881_instr( code_ptr )
short *code_ptr;
{ os_err = "No M68881 emulation";
  return 1;
}

#else

#include <math.h>

int matherr( x )
struct exception *x;
{ return 1; /* don't trap on error */
}

void fp_mov_l_to_fp0()   { pstate->fp0 = pstate->temp[0]; }
void fp_mov_l_from_fp0() { pstate->temp[0] = pstate->fp0; }

void fp_mov_d_to_fp0()   { pstate->fp0 = *pstate->fp_ptr; }
void fp_mov_d_from_fp0() { *pstate->fp_ptr = pstate->fp0; }

void fp_abs_to_fp0()     { pstate->fp0 = fabs( (double)*pstate->fp_ptr ); }
void fp_abs_from_fp0()   { *pstate->fp_ptr = fabs( (double)pstate->fp0 ); }

void fp_int_to_fp0()
{ double x = *pstate->fp_ptr;
  double y = fmod( x, 2.0 );
  if (y == 0.5)
    pstate->fp0 = x-0.5;
  else if (y == -1.5)
    pstate->fp0 = x-0.5;
  else 
    pstate->fp0 = floor( 0.5+x );
}

void fp_int_from_fp0()
{ double x = pstate->fp0;
  double y = fmod( x, 2.0 );
  if (y == 0.5)
    *pstate->fp_ptr = x-0.5;
  else if (y == -1.5)
    *pstate->fp_ptr = x-0.5;
  else 
    *pstate->fp_ptr = floor( 0.5+x );
}

void fp_intrz_to_fp0()
{ if (*pstate->fp_ptr < 0.0)
    pstate->fp0 = -floor( -(double)*pstate->fp_ptr );
  else
    pstate->fp0 = floor( (double)*pstate->fp_ptr );
}

void fp_intrz_from_fp0()
{ if (pstate->fp0 < 0.0)
    *pstate->fp_ptr = -floor( -(double)pstate->fp0 );
  else
    *pstate->fp_ptr = floor( (double)pstate->fp0 );
}

void fp_sqrt_to_fp0()    { pstate->fp0 = sqrt( (double)*pstate->fp_ptr ); }
void fp_sqrt_from_fp0()  { *pstate->fp_ptr = sqrt( (double)pstate->fp0 ); }

void fp_etox_to_fp0()    { pstate->fp0 = exp( (double)*pstate->fp_ptr ); }
void fp_etox_from_fp0()  { *pstate->fp_ptr = exp( (double)pstate->fp0 ); }

void fp_logn_to_fp0()    { pstate->fp0 = log( (double)*pstate->fp_ptr ); }
void fp_logn_from_fp0()  { *pstate->fp_ptr = log( (double)pstate->fp0 ); }

void fp_sin_to_fp0()     { pstate->fp0 = sin( (double)*pstate->fp_ptr ); }
void fp_sin_from_fp0()   { *pstate->fp_ptr = sin( (double)pstate->fp0 ); }

void fp_cos_to_fp0()     { pstate->fp0 = cos( (double)*pstate->fp_ptr ); }
void fp_cos_from_fp0()   { *pstate->fp_ptr = cos( (double)pstate->fp0 ); }

void fp_tan_to_fp0()     { pstate->fp0 = tan( (double)*pstate->fp_ptr ); }
void fp_tan_from_fp0()   { *pstate->fp_ptr = tan( (double)pstate->fp0 ); }

void fp_asin_to_fp0()    { pstate->fp0 = asin( (double)*pstate->fp_ptr ); }
void fp_asin_from_fp0()  { *pstate->fp_ptr = asin( (double)pstate->fp0 ); }

void fp_acos_to_fp0()    { pstate->fp0 = acos( (double)*pstate->fp_ptr ); }
void fp_acos_from_fp0()  { *pstate->fp_ptr = acos( (double)pstate->fp0 ); }

void fp_atan_to_fp0()    { pstate->fp0 = atan( (double)*pstate->fp_ptr ); }
void fp_atan_from_fp0()  { *pstate->fp_ptr = atan( (double)pstate->fp0 ); }

void fp_add_to_fp0()     { pstate->fp0 = pstate->fp0 + *pstate->fp_ptr; }
void fp_add_from_fp0()   { *pstate->fp_ptr = *pstate->fp_ptr + pstate->fp0; }

void fp_sub_to_fp0()     { pstate->fp0 = pstate->fp0 - *pstate->fp_ptr; }
void fp_sub_from_fp0()   { *pstate->fp_ptr = *pstate->fp_ptr - pstate->fp0; }

void fp_mul_to_fp0()     { pstate->fp0 = pstate->fp0 * *pstate->fp_ptr; }
void fp_mul_from_fp0()   { *pstate->fp_ptr = *pstate->fp_ptr * pstate->fp0; }

void fp_div_to_fp0()     { pstate->fp0 = pstate->fp0 / *pstate->fp_ptr; }
void fp_div_from_fp0()   { *pstate->fp_ptr = *pstate->fp_ptr / pstate->fp0; }

void fp_cmp_to_fp0()     { pstate->fp_cmp1 = *pstate->fp_ptr; pstate->fp_cmp2 = pstate->fp0; }
void fp_cmp_from_fp0()   { pstate->fp_cmp1 = pstate->fp0; pstate->fp_cmp2 = *pstate->fp_ptr; }


void fp_beq()
{ if (pstate->fp_cmp2 == pstate->fp_cmp1) pstate->temp[3] += pstate->temp[0];
}


void fp_bne()
{ if (pstate->fp_cmp2 != pstate->fp_cmp1) pstate->temp[3] += pstate->temp[0];
}


void fp_blt()
{ if (pstate->fp_cmp2 < pstate->fp_cmp1) pstate->temp[3] += pstate->temp[0];
}


void fp_bgt()
{ if (pstate->fp_cmp2 > pstate->fp_cmp1) pstate->temp[3] += pstate->temp[0];
}


void fp_ble()
{ if (pstate->fp_cmp2 <= pstate->fp_cmp1) pstate->temp[3] += pstate->temp[0];
}


void fp_bge()
{ if (pstate->fp_cmp2 >= pstate->fp_cmp1) pstate->temp[3] += pstate->temp[0];
}


long emul_M68881_instr( code_ptr )
short *code_ptr;
{ short *p1, *p2, *p3;
  gen_ptr = gen;
  code_len = 2;
  *gen_ptr++ = MOVE_L_PINCA7_A5_DISP_OP;
  *gen_ptr++ = pstate_offset( &pstate->temp[3] );
  switch (code_ptr[0] & 0xffc0)
  { case 0xf200: /* floating point operation */
    { long ea, f_reg, op;
      ea = code_ptr[0] & 0x003f;
      op = code_ptr[1] & 0x003f;
      f_reg = (code_ptr[1] & 0x0380) >> 7;
      if (f_reg != 0) goto error;
      if (op == 0) /* fmov ? */
        switch (code_ptr[1] & 0xfc00)
        { case 0x4000: /* fmov.l <ea>,fp0 */
            *gen_ptr++ = MOVE_L_EA_A5_DISP_OP+ea;
            gen_ea( ea, code_ptr );
            *gen_ptr++ = pstate_offset( &pstate->temp[0] );
            *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
            *(void (**)())gen_ptr = fp_mov_l_to_fp0;  gen_ptr += 2;
            *gen_ptr++ = JSRA6_DISP_OP;
            *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
            break;
          case 0x5400: /* fmov.d <ea>,fp0 */
            *gen_ptr++ = PEA_OP+ea;
            gen_ea( ea, code_ptr );
            *gen_ptr++ = MOVE_L_PINCA7_A5_DISP_OP;
            *gen_ptr++ = pstate_offset( &pstate->fp_ptr );
            *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
            *(void (**)())gen_ptr = fp_mov_d_to_fp0;  gen_ptr += 2;
            *gen_ptr++ = JSRA6_DISP_OP;
            *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
            break;
          case 0x6000: /* fmov.l fp0,<ea> */
            *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
            *(void (**)())gen_ptr = fp_mov_l_from_fp0;  gen_ptr += 2;
            *gen_ptr++ = JSRA6_DISP_OP;
            *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
            *gen_ptr++ = MOVE_L_A5_DISP_EA_OP+((ea&0x07)<<9)+((ea&0x38)<<3);
            *gen_ptr++ = pstate_offset( &pstate->temp[0] );
            gen_ea( ea, code_ptr );
            break;
          case 0x7400: /* fmov.d fp0,<ea> */
            *gen_ptr++ = PEA_OP+ea;
            gen_ea( ea, code_ptr );
            *gen_ptr++ = MOVE_L_PINCA7_A5_DISP_OP;
            *gen_ptr++ = pstate_offset( &pstate->fp_ptr );
            *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
            *(void (**)())gen_ptr = fp_mov_d_from_fp0;  gen_ptr += 2;
            *gen_ptr++ = JSRA6_DISP_OP;
            *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
            break;
          default:
            goto error;
          }
      else
      { if ((code_ptr[1] & 0xdc00) != 0x5400) goto error;
        *gen_ptr++ = PEA_OP+ea;
        gen_ea( ea, code_ptr );
        *gen_ptr++ = MOVE_L_PINCA7_A5_DISP_OP;
        *gen_ptr++ = pstate_offset( &pstate->fp_ptr );
        *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
        if ((code_ptr[1] & 0x2000) == 0)
          switch (op)
          { case 0x01: *(void (**)())gen_ptr = fp_int_to_fp0;   break;
            case 0x03: *(void (**)())gen_ptr = fp_intrz_to_fp0; break;
            case 0x04: *(void (**)())gen_ptr = fp_sqrt_to_fp0;  break;
            case 0x0a: *(void (**)())gen_ptr = fp_atan_to_fp0;  break;
            case 0x0c: *(void (**)())gen_ptr = fp_asin_to_fp0;  break;
            case 0x0e: *(void (**)())gen_ptr = fp_sin_to_fp0;   break;
            case 0x0f: *(void (**)())gen_ptr = fp_tan_to_fp0;   break;
            case 0x10: *(void (**)())gen_ptr = fp_etox_to_fp0;  break;
            case 0x14: *(void (**)())gen_ptr = fp_logn_to_fp0;  break;
            case 0x18: *(void (**)())gen_ptr = fp_abs_to_fp0;   break;
            case 0x1c: *(void (**)())gen_ptr = fp_acos_to_fp0;  break;
            case 0x1d: *(void (**)())gen_ptr = fp_cos_to_fp0;   break;
            case 0x20: *(void (**)())gen_ptr = fp_div_to_fp0;   break;
            case 0x22: *(void (**)())gen_ptr = fp_add_to_fp0;   break;
            case 0x23: *(void (**)())gen_ptr = fp_mul_to_fp0;   break;
            case 0x28: *(void (**)())gen_ptr = fp_sub_to_fp0;   break;
            case 0x38: *(void (**)())gen_ptr = fp_cmp_to_fp0;   break;
            default:   goto error;
          }
        else
          switch (op)
          { case 0x01: *(void (**)())gen_ptr = fp_int_from_fp0;   break;
            case 0x03: *(void (**)())gen_ptr = fp_intrz_from_fp0; break;
            case 0x04: *(void (**)())gen_ptr = fp_sqrt_from_fp0;  break;
            case 0x0a: *(void (**)())gen_ptr = fp_atan_from_fp0;  break;
            case 0x0c: *(void (**)())gen_ptr = fp_asin_from_fp0;  break;
            case 0x0e: *(void (**)())gen_ptr = fp_sin_from_fp0;   break;
            case 0x0f: *(void (**)())gen_ptr = fp_tan_from_fp0;   break;
            case 0x10: *(void (**)())gen_ptr = fp_etox_from_fp0;  break;
            case 0x14: *(void (**)())gen_ptr = fp_logn_from_fp0;  break;
            case 0x18: *(void (**)())gen_ptr = fp_abs_from_fp0;   break;
            case 0x1c: *(void (**)())gen_ptr = fp_acos_from_fp0;  break;
            case 0x1d: *(void (**)())gen_ptr = fp_cos_from_fp0;   break;
            case 0x20: *(void (**)())gen_ptr = fp_div_from_fp0;   break;
            case 0x22: *(void (**)())gen_ptr = fp_add_from_fp0;   break;
            case 0x23: *(void (**)())gen_ptr = fp_mul_from_fp0;   break;
            case 0x28: *(void (**)())gen_ptr = fp_sub_from_fp0;   break;
            case 0x38: *(void (**)())gen_ptr = fp_cmp_from_fp0;   break;
            default:   goto error;
          }
        gen_ptr += 2;
        *gen_ptr++ = JSRA6_DISP_OP;
        *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
      }
      break;
    }
    case 0xf280: /* floating point conditional branch */
    { long disp = ((long)code_ptr[1])-2;
      *gen_ptr++ = MOVE_L_IMM_A5_DISP_OP;
      *gen_ptr++ = (disp >> 16) & 0xffff;
      *gen_ptr++ = disp & 0xffff;
      *gen_ptr++ = pstate_offset( &pstate->temp[0] );
      *gen_ptr++ = MOVE_L_IMM_PDECA7_OP;
      switch (code_ptr[0] & 0x003f)
      { case 0x01: *(void (**)())gen_ptr = fp_beq; break;
        case 0x0e: *(void (**)())gen_ptr = fp_bne; break;
        case 0x14: *(void (**)())gen_ptr = fp_blt; break;
        case 0x12: *(void (**)())gen_ptr = fp_bgt; break;
        case 0x15: *(void (**)())gen_ptr = fp_ble; break;
        case 0x13: *(void (**)())gen_ptr = fp_bge; break;
        default:   goto error;
      }
      gen_ptr += 2;
      *gen_ptr++ = JSRA6_DISP_OP;
      *gen_ptr++ = table_offset( &sstate->traps[C_TRAP_trap].jmp );
      break;
    }
    default:
      goto error;
  }
  *gen_ptr++ = MOVE_L_A5_DISP_PDECA7_OP;
  *gen_ptr++ = pstate_offset( &pstate->temp[3] );
  *gen_ptr++ = RTS_OP;
  p1 = (short *)pstate->emul_code_bot;
  while (p1 < (short *)emul_code_alloc)
  { p2 = p1;
    p3 = gen;
    while ((p3 < gen_ptr) && (*p2 == *p3)) { p2++; p3++; }
    if (p3 == gen_ptr)
    { *code_ptr++ = JSRA5_DISP_OP;
      *code_ptr++ = pstate_offset( p1 );
      while (code_len > 2) { *code_ptr++ = NOP_OP; code_len--; }
      return 0;
    }
    p1++;
  }
  if (emul_code_alloc+(gen_ptr-gen)*sizeof(short) > emul_code_top)
  { os_err = "Emulation code memory overflow"; return 1; }
  *code_ptr++ = JSRA5_DISP_OP;
  *code_ptr++ = pstate_offset( emul_code_alloc );
  while (code_len > 2) { *code_ptr++ = NOP_OP; code_len--; }
  p1 = gen;
  while (p1 < gen_ptr)
  { *(short *)emul_code_alloc = *p1++;
    emul_code_alloc += sizeof(short);
  }
  return 0;
  error:
  os_err = "Unknown M68881 instruction";
  return 1;
}

#endif

/*---------------------------------------------------------------------------*/