/*
 * Copyright (C) 1996-1998 by the Board of Trustees
 *    of Leland Stanford Junior University.
 * 
 * This file is part of the SimOS distribution. 
 * See LICENSE file for terms of the license. 
 *
 */

/*****************************************************************
 * fpu.c
 * 
 * $Author: blythe $
 * $Date: 2002/05/29 01:09:10 $
 *****************************************************************/
#include <math.h>
#include <limits.h>
#ifdef __alpha
#include <float.h>
#else
# ifndef i386
# include <ieeefp.h>
# endif
#endif
#include "mipsy.h"
#include "cpu.h"
#include "cpu_state.h"
#include "sim_error.h"
#include "machine_params.h"
#include "cp0.h"
#include "fpu.h"
#include "cpu_stats.h"

extern Result MipsyPrefetch(CPUState *P, VA vAddr, uint hint);
/*
 * CHECK_64BIT_ALLOWED() - Check to see if 64bit instructions are permitted,
 *                         if not raise an exception. 
 */
#if defined(SIM_MIPS64)
#define CHECK_64BIT_ALLOWED(_p)  \
      if ((_p)->is32bitMode && !IS_KERNEL_MODE((_p))) {  \
         EXCEPTION((_p), EXC_II);                 \
         return C1_CONTINUE;                             \
      }
#else
#define CHECK_64BIT_ALLOWED(_p) \
      if (1) {                                           \
         EXCEPTION((_p), EXC_II);                 \
         return C1_CONTINUE;                             \
      }

#endif

/* Enumeration of floating point exception types */
typedef enum { FPE_I = 0, FPE_U, FPE_O, FPE_Z, FPE_V, FPE_E, FPECount } FPUException;
static int FPCSRFlag[FPECount] = {
   0x00000004, 0x00000008, 0x00000010, 0x00000020, 0x00000040, 0x00000000
};
static int FPCSREnable[FPECount] = {
   0x00000080, 0x00000100, 0x00000200, 0x00000400, 0x00000800, 0x00000000
};
static int FPCSRCause[FPECount] = {
   0x00001000, 0x00002000, 0x00004000, 0x00008000, 0x00010000, 0x00020000
};

/*****************************************************************
 * WARNING: An ugly feature of the sgi IEEE fp implementation greatly
 * complicates this c-based fpu model. FP operations on doubles take
 * place in a reversed order in the registers than they would in
 * memory. As a result, operations on doubles in my memory-based fp
 * registers would all be word-swapped. As a result, I swap every
 * single float reference so that reg 1 is in reg 0's place in memory
 * and vice versa. It's a pain, but will speed up the double
 * operations. 
 * 
 * There are a few side effects of this change:
 * -> The "real" fpu state must be copied into mipsy's special
 *    structure and out of it on entry and exit.
 * -> Gdb get reg needs to be aware of the special state
 * -> People debugging the fpu should be aware that FPR doesn't
 *    have the most up-to-date copy!!!
 ****************************************************************/
 
void
MipsyInitFPU(void)
{
   CPUState *P;
   int i,j;
   
   for (i=0; i < TOTAL_CPUS; i++) {
      P = &PE[i];
      for (j=0; j < NUM_FP_REGS; j++) {
#ifdef NEW_FP_REGS
         LongLongReg(j) = P->FPR[j];
#else
         IntReg(j) = P->FPR[j];
#endif
      }
   }
}

void 
MipsyExitFPU(void)
{
   CPUState *P;
   int i,j;
   
   for (i=0; i < TOTAL_CPUS; i++) {
      P = &PE[i];
      for (j=0; j < NUM_FP_REGS; j++) {
#ifdef NEW_FP_REGS
         P->FPR[j] = LongLongReg(j);
#else
         P->FPR[j] = IntReg(j);
#endif
      }
   }
}

static Result
MipsyFPUException(CPUState *P, FPUException type)
{
   P->FCR[31] |= FPCSRFlag[type];
   P->FCR[31] |= FPCSRCause[type];
   if (P->FCR[31] & FPCSREnable[type]) {
      EXCEPTION(P, EXC_FPE);
      return C1_CONTINUE;
   } else {
      return C1_SUCCESS;
   }
}

/*****************************************************************
 * MipsyFPU
 *****************************************************************/
static Result 
MipsyFPU(CPUState *P, Inst instr) 
{
   uint fmt = FORMAT(instr);
   uint ft  = FT(instr); /* 2nd source register */
   uint fs  = FS(instr); /* 1st source reg */
   uint fd  = FD(instr); /* Destination */
   uint func = FUNC(instr);
   StatusReg statusReg;

   /* Clear the exception flags from the FCR */
   P->FCR[31] &= ~FPCSR_EXCEPTIONS;
   
   switch(func+(fmt<<8)) {
      
   case fabs_op+F_SSINGLE:
      FloatReg(fd) = (FloatReg(fs) >= 0.0 ? FloatReg(fs) : -FloatReg(fs));
      SIM_DEBUG(('f', "C1:ABS.S: f%d %e becomes f%d %e\n", fs, FloatReg(fs), fd, 
                 FloatReg(fd)));
      break;
   case fabs_op+F_SDOUBLE:
      DoubleReg(fd) = (DoubleReg(fs) >= 0.0 ? DoubleReg(fs) : -DoubleReg(fs));
      SIM_DEBUG(('f', "C1:ABS.D: f%d %Le becomes f%d %Le\n", fs, DoubleReg(fs), fd, 
                 DoubleReg(fd)));
      break;
      
   case fadd_op+F_SSINGLE:
      FloatReg(fd) = FloatReg(fs) + FloatReg(ft);
      SIM_DEBUG(('f', "C1:ADD.S: f%d %e + f%d %e = f%d %e\n", fs, FloatReg(fs), ft, 
                 FloatReg(ft), fd, FloatReg(fd)));
      break;
   case fadd_op+F_SDOUBLE:
      DoubleReg(fd) = DoubleReg(fs) + DoubleReg(ft);
      SIM_DEBUG(('f', "C1:ADD.D: f%d %Le + f%d %Le = f%d %Le\n", fs, DoubleReg(fs), ft, 
                 DoubleReg(ft), fd, DoubleReg(fd)));
      break;

   case fcvtd_op+F_SSINGLE:
      DoubleReg(fd) = FloatReg(fs);
      SIM_DEBUG(('f', "C1:CVTD.S: f%d %e becomes f%d %Le\n", fs, FloatReg(fs), fd, 
                 DoubleReg(fd))); 
      break;
   case fcvtd_op+F_W: {
      int old = IntReg(fs);

      DoubleReg(fd) = (double)old;
      SIM_DEBUG(('f', "C1:CVTD.W: f%d %d becomes f%d %Le\n", fs, old, fd, 
                 DoubleReg(fd)));
      }
      break;

   case fcvts_op+F_SDOUBLE:
      FloatReg(fd) = DoubleReg(fs);
      SIM_DEBUG(('f', "C1:CVTS.D: f%d %Le becomes f%d %e\n", fs, DoubleReg(fs), fd, 
            FloatReg(fd)));
      break;
     
   case fcvts_op+F_W:
      FloatReg(fd) = IntReg(fs);
      SIM_DEBUG(('f', "C1:CVTS.W: f%d %d becomes f%d %e\n", fs, IntReg(fs), fd, 
                FloatReg(fd)));
      break;

   case fcvts_op+F_L:
      FloatReg(fd) = LongLongReg(fs);
      /*CPUWarning("MIPSY: cvt.s.l @%#llx (partial impl.)\n", P->PC);*/
      SIM_DEBUG(('f', "C1:CVTS.L: f%d %lld becomes f%d %e\n", fs,
		 LongLongReg(fs), fd, FloatReg(fd)));
      break;

   case fcvtd_op+F_L:
      DoubleReg(fd) = LongLongReg(fs);
      /*CPUWarning("MIPSY: cvt.d.l @%#llx (partial impl.)\n", P->PC);*/
      SIM_DEBUG(('f', "C1:CVTD.L: f%d %lld becomes f%d %Le\n", fs,
		 LongLongReg(fs), fd, DoubleReg(fd)));
      break;

   case fcvtl_op+F_SSINGLE: {
      uint roundMode = P->FCR[31] & 0x03;
      CHECK_64BIT_ALLOWED(P);
      switch (roundMode) {
      case 0:                 /* RN: Round to nearest         */
         LongLongReg(fd) = (FloatReg(fs) > 0.0 ? FloatReg(fs) + 0.5 : FloatReg(fs) - 0.5 );
         break;
         
      case 1:                 /* RZ: Round toward 0           */
         LongLongReg(fd) = FloatReg(fs);
         break;
         
      case 2:                 /* RP: Round to +infinity       */
         LongLongReg(fd) = (FloatReg(fs) >= 0.0 ? 
                     FloatReg(fs) + SP_NEAR_ONE : 
                     FloatReg(fs));
         break;
         
      case 3:                 /* RM: Round to -infinity       */
         LongLongReg(fd) = (FloatReg(fs) >= 0.0 ? 
                     FloatReg(fs) :
                     FloatReg(fs) - SP_NEAR_ONE);
         break;
      }

      SIM_DEBUG(('f', "C1:CVTW.S: f%d %e becomes f%d %d rm = %d\n", 
                 fs, FloatReg(fs), fd, IntReg(fd), roundMode));
      break;
   }
      
   case fcvtl_op+F_SDOUBLE: {
      uint roundMode = P->FCR[31] & 0x03;
      CHECK_64BIT_ALLOWED(P);
      switch (roundMode) {
      case 0:                 /* RN: Round to nearest         */
         LongLongReg(fd) = (DoubleReg(fs) > 0.0 ? DoubleReg(fs) + 0.5 : DoubleReg(fs) - 0.5 );
         break;
         
      case 1:                 /* RZ: Round toward 0           */
         LongLongReg(fd) = DoubleReg(fs);
         break;
         
      case 2:                 /* RP: Round to +infinity       */
         LongLongReg(fd) = (DoubleReg(fs) >= 0.0 ? 
                     DoubleReg(fs) + DP_NEAR_ONE : 
                     DoubleReg(fs));
         break;
         
      case 3:                 /* RM: Round to -infinity       */
         LongLongReg(fd) = (DoubleReg(fs) >= 0.0 ? 
                     DoubleReg(fs) :
                     DoubleReg(fs) - DP_NEAR_ONE);
         break;
      }
      SIM_DEBUG(('f', "C1:CVTL.D: f%d %Le becomes f%d %d rm = %d\n", 
                 fs, DoubleReg(fs), fd, IntReg(fd), roundMode));
      break;
   }

   case fcvtw_op+F_SSINGLE: {
      uint roundMode = P->FCR[31] & 0x03;
      float rounded;
      switch (roundMode) {
      case 0:                 /* RN: Round to nearest         */
         rounded = (FloatReg(fs) > 0.0 ? FloatReg(fs) + 0.5 : FloatReg(fs) - 0.5 );
         break;
         
      case 1:                 /* RZ: Round toward 0           */
         rounded = FloatReg(fs);
         break;
         
      case 2:                 /* RP: Round to +infinity       */
         rounded = (FloatReg(fs) >= 0.0 ? 
                     FloatReg(fs) + SP_NEAR_ONE : 
                     FloatReg(fs));
         break;
         
      case 3:                 /* RM: Round to -infinity       */
         rounded = (FloatReg(fs) >= 0.0 ? 
                     FloatReg(fs) :
                     FloatReg(fs) - SP_NEAR_ONE);
         break;
      }

      IntReg(fd) = rounded;
      SIM_DEBUG(('f', "C1:CVTW.S: f%d %e becomes f%d %d rm = %d\n", 
                 fs, FloatReg(fs), fd, IntReg(fd), roundMode));

      /* Check for valid operand */
      if ((rounded > (float)INT_MAX) || (rounded < (float)INT_MIN)) {
         int ret = MipsyFPUException(P, FPE_V);
         return ret;
      }
      break;
   }
      
   case fcvtw_op+F_SDOUBLE: {
      uint roundMode = P->FCR[31] & 0x03;
      double rounded;

      switch (roundMode) {
      case 0:                 /* RN: Round to nearest         */
         rounded = (DoubleReg(fs) > 0.0 ? DoubleReg(fs) + 0.5 : DoubleReg(fs) - 0.5 );
         break;
         
      case 1:                 /* RZ: Round toward 0           */
         rounded = DoubleReg(fs);
         break;
         
      case 2:                 /* RP: Round to +infinity       */
         rounded = (DoubleReg(fs) >= 0.0 ? 
                     DoubleReg(fs) + DP_NEAR_ONE : 
                     DoubleReg(fs));
         break;
         
      case 3:                 /* RM: Round to -infinity       */
         rounded = (DoubleReg(fs) >= 0.0 ? 
                     DoubleReg(fs) :
                     DoubleReg(fs) - DP_NEAR_ONE);
         break;
      }

      IntReg(fd) = rounded;
      SIM_DEBUG(('f', "C1:CVTW.D: f%d %Le becomes f%d %d rm = %d\n", 
                 fs, DoubleReg(fs), fd, IntReg(fd), roundMode));


      /* Check for valid operand */
      if ((rounded > (double)INT_MAX) || (rounded < (double)INT_MIN)) {
         int ret = MipsyFPUException(P, FPE_V);
         return ret;
      }

      break;
   }

   case fdiv_op+F_SSINGLE:
      if (FloatReg(ft) == 0.0) {
         CPUWarning("MIPSY: FDIV divide by zero @%#llx\n", P->PC);
      } 
      FloatReg(fd) = FloatReg(fs) / FloatReg(ft);
      SIM_DEBUG(('f', "C1:FDIV.S: f%d %e / f%d %e = f%d %e\n", 
                 fs, FloatReg(fs), ft, FloatReg(ft), fd, FloatReg(fd)));
      break;
   case fdiv_op+F_SDOUBLE:
      if (DoubleReg(ft) == 0.0) {
         CPUWarning("MIPSY: FDIV divide by zero\n");
      } 
      DoubleReg(fd) = DoubleReg(fs) / DoubleReg(ft);
      SIM_DEBUG(('f', "C1:FDIV.D f%d %Le / f%d %Le = f%d %Le\n", fs, DoubleReg(fs), ft, 
                 DoubleReg(ft), fd, DoubleReg(fd)));
      break;

   case fmov_op+F_SSINGLE:
      FloatReg(fd) = FloatReg(fs);
      SIM_DEBUG(('f', "C1:FMOV.S %e to %e\n", FloatReg(fs), 
                 FloatReg(fd)));
      break;  
   case fmov_op+F_SDOUBLE:
      DoubleReg(fd) = DoubleReg(fs);
      SIM_DEBUG(('f', "C1:FMOV.D %Le to %Le\n", DoubleReg(fs), 
                 DoubleReg(fd)));
      break;  

   case fmovc_op+F_SSINGLE:
      /* Move conditional on FP condition */
      if (TF(instr)) { 
         if (P->FCR[31] & M_csr_c) {
            FloatReg(fd) = FloatReg(fs);
         }
      } else {
         if (!(P->FCR[31] & M_csr_c)) {
            FloatReg(fd) = FloatReg(fs);
         }
      }
      break;
   case fmovc_op+F_SDOUBLE:
      /* Move conditional on FP condition */
      if (TF(instr)) { 
         if (P->FCR[31] & M_csr_c) {
            DoubleReg(fd) = DoubleReg(fs);
         }
      } else {
         if (!(P->FCR[31] & M_csr_c)) {
            DoubleReg(fd) = DoubleReg(fs);
         }
      }
      break;

   case fmovn_op+F_SSINGLE:
      if (P->R[ft] != 0) { 
         FloatReg(fd) = FloatReg(fs);
      }
      SIM_DEBUG(('f', "C1:FMOVN.S %e to %e\n", FloatReg(fs), FloatReg(fd)));
      break;  
   case fmovn_op+F_SDOUBLE:
      if (P->R[ft] != 0) { 
         DoubleReg(fd) = DoubleReg(fs);
      }
      SIM_DEBUG(('f', "C1:FMOVN.D %Le to %Le\n", DoubleReg(fs), DoubleReg(fd)));
      break;  

   case fmovz_op+F_SSINGLE:
      if (P->R[ft] == 0) { 
         FloatReg(fd) = FloatReg(fs);
      }
      SIM_DEBUG(('f', "C1:FMOVZ.S %e to %e\n", FloatReg(fs), FloatReg(fd)));
      break;  
   case fmovz_op+F_SDOUBLE:
      if (P->R[ft] == 0) { 
         DoubleReg(fd) = DoubleReg(fs);
      }
      SIM_DEBUG(('f', "C1:FMOVZ.D %Le to %Le\n", DoubleReg(fs), DoubleReg(fd)));
      break;  

   case fmul_op+F_SSINGLE:
      FloatReg(fd) = FloatReg(fs) * FloatReg(ft);
      SIM_DEBUG(('f', "C1:FMUL.S: f%d %e * f%d %e = f%d %e\n", 
                 fs, FloatReg(fs), ft, FloatReg(ft), fd, FloatReg(fd)));
      break;
   case fmul_op+F_SDOUBLE:
      DoubleReg(fd) = DoubleReg(fs) * DoubleReg(ft);
      SIM_DEBUG(('f', "C1:FMUL.D: f%d %Le * f%d %Le = f%d %Le\n", fs, DoubleReg(fs), ft, 
                 DoubleReg(ft), fd, DoubleReg(fd)));
      break;

   case fneg_op+F_SSINGLE:
      FloatReg(fd) = -FloatReg(fs);
      SIM_DEBUG(('f', "C1:NEG.S: f%d %e becomes f%d %e\n", 
                 fs, FloatReg(fs), fd, FloatReg(fd)));
      break;
   case fneg_op+F_SDOUBLE:
      DoubleReg(fd) = -DoubleReg(fs);
      SIM_DEBUG(('f', "C1:NEG.D: f%d %Le becomes f%d %Le\n", 
                 fs, FloatReg(fs), fd, FloatReg(fd)));
      break;

   case fsub_op+F_SSINGLE:
      FloatReg(fd) = FloatReg(fs) - FloatReg(ft);
      SIM_DEBUG(('f', "C1:SUB.S: f%d %e - f%d %e = f%d %e\n", 
                 fs, FloatReg(fs), ft, FloatReg(ft), fd, FloatReg(fd)));
      break;
   case fsub_op+F_SDOUBLE:
      DoubleReg(fd) = DoubleReg(fs) - DoubleReg(ft);
      SIM_DEBUG(('f', "C1:SUB.D: f%d %Le - f%d %Le = f%d %Le\n", 
                 fs, DoubleReg(fs), ft, DoubleReg(ft), fd, DoubleReg(fd)));
      break;
    
   case fsqrt_op+F_SSINGLE:	
      FloatReg(fd) = (float)sqrt((double)FloatReg(fs));
      break;
   case fsqrt_op+F_SDOUBLE:
      DoubleReg(fd) = sqrt (DoubleReg(fs));
      break;

   case ftrunc_op+F_SSINGLE:
      IntReg(fd) = FloatReg(fs);
      break;
   case ftrunc_op+F_SDOUBLE:
      IntReg(fd) = DoubleReg(fs);
      break;

   case ftruncl_op+F_SSINGLE:
      CHECK_64BIT_ALLOWED(P);
      LongLongReg(fd) = FloatReg(fs);
      break;
   case ftruncl_op+F_SDOUBLE:
      CHECK_64BIT_ALLOWED(P);
      LongLongReg(fd) = DoubleReg(fs);
      break;

   case ffloorl_op+F_SSINGLE:
      IntReg(fd) = (FloatReg(fs) >= 0.0 ? FloatReg(fs) : FloatReg(fs) - SP_NEAR_ONE );
      break;
   case ffloorl_op+F_SDOUBLE:
      IntReg(fd) = (DoubleReg(fs) >= 0.0 ? DoubleReg(fs) : DoubleReg(fs) - DP_NEAR_ONE );
      break;

   case frecip_op+F_SSINGLE: {
      statusReg.ts_data = P->CP0[C0_SR];
      /* Make sure we can execute mips4 instructions */
      if (IS_USER_MODE(P) && !(statusReg.s32.ts_xx)) {
         EXCEPTION(P, EXC_II); 
         return C1_CONTINUE;
      }
      if (FloatReg(fs) == 0.0) {
         CPUWarning("MIPSY: FDIV divide by zero\n");
      } 
      FloatReg(fd) = 1.0 / FloatReg(fs);
      SIM_DEBUG(('f', "C1:FRECIP.S: f%d %e / f%d %e = f%d %e\n", 
                 fs, FloatReg(fs), ft, FloatReg(ft), fd, FloatReg(fd)));
      break;
   }
   case frecip_op+F_SDOUBLE: {
      statusReg.ts_data = P->CP0[C0_SR];
      /* Make sure we can execute mips4 instructions */
      if (IS_USER_MODE(P) && !(statusReg.s32.ts_xx)) {
         EXCEPTION(P, EXC_II); 
         return C1_CONTINUE;
      }
      if (DoubleReg(fs) == 0.0) {
         CPUWarning("MIPSY: FDIV divide by zero\n");
      } 
      DoubleReg(fd) = 1.0 / DoubleReg(fs);
      SIM_DEBUG(('f', "C1:FRECIP.D f%d %Le / f%d %Le = f%d %Le\n", fs, DoubleReg(fs), ft, 
                 DoubleReg(ft), fd, DoubleReg(fd)));
      break;
   }
   case frsqrt_op+F_SSINGLE:
      statusReg.ts_data = P->CP0[C0_SR];
      /* Make sure we can execute mips4 instructions */
      if (IS_USER_MODE(P) && !(statusReg.s32.ts_xx)) {
         EXCEPTION(P, EXC_II); 
         return C1_CONTINUE;
      }
      if (FloatReg(fs) == 0.0) {
         CPUWarning("MIPSY: FRSQRT divide by zero\n");
      } 
      FloatReg(fd) = 1.0 / (float)sqrt((double)FloatReg(fs));
      break;
      
   case frsqrt_op+F_SDOUBLE:
      statusReg.ts_data = P->CP0[C0_SR];
      /* Make sure we can execute mips4 instructions */
      if (IS_USER_MODE(P) && !(statusReg.s32.ts_xx)) {
         EXCEPTION(P, EXC_II); 
         return C1_CONTINUE;
      }
      if (DoubleReg(fs) == 0.0) {
         CPUWarning("MIPSY: FDIV divide by zero\n");
      } 
      DoubleReg(fd) = 1.0 / sqrt(DoubleReg(fs));
      break;

   case fround_op+F_SSINGLE:
      IntReg(fd) = (FloatReg(fs) > 0.0 ? FloatReg(fs) + 0.5 : FloatReg(fs) - 0.5 );
      break;
   case fround_op+F_SDOUBLE:
      IntReg(fd) = (DoubleReg(fs) > 0.0 ? DoubleReg(fs) + 0.5 : DoubleReg(fs) - 0.5 );
      break;

   case froundl_op+F_SSINGLE:
      CHECK_64BIT_ALLOWED(P);
      LongLongReg(fd) = (FloatReg(fs) > 0.0 ? FloatReg(fs) + 0.5 : FloatReg(fs) - 0.5 );
      break;
   case froundl_op+F_SDOUBLE:
      CHECK_64BIT_ALLOWED(P);
      LongLongReg(fd) = (DoubleReg(fs) > 0.0 ? DoubleReg(fs) + 0.5 : DoubleReg(fs) - 0.5 );
      break;

   case fceil_op+F_SSINGLE:
      IntReg(fd) = (FloatReg(fs) >= 0.0 ? FloatReg(fs) + SP_NEAR_ONE : FloatReg(fs) );
      break;
   case fceil_op+F_SDOUBLE:
      IntReg(fd) = (DoubleReg(fs) >= 0.0 ? DoubleReg(fs) + DP_NEAR_ONE : DoubleReg(fs) );
      break;

   case fceill_op+F_SSINGLE:
      CHECK_64BIT_ALLOWED(P);
      LongLongReg(fd) = (FloatReg(fs) >= 0.0 ? FloatReg(fs) + SP_NEAR_ONE : FloatReg(fs) );
      break;
   case fceill_op+F_SDOUBLE:
      CHECK_64BIT_ALLOWED(P);
      LongLongReg(fd) = (DoubleReg(fs) >= 0.0 ? DoubleReg(fs) + DP_NEAR_ONE : DoubleReg(fs) );
      break;

      /* floating point comparison operations */

   case F_C_EQ+F_SSINGLE:
      if (FloatReg(fs) == FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_EQ+F_SDOUBLE:
      if (DoubleReg(fs) == DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_NGL+F_SSINGLE:
      if (FloatReg(fs) == FloatReg(ft))
         IntReg(fd) |= CONDBIT;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_NGL+F_SDOUBLE:
      if (DoubleReg(fs) == DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
      
   case F_C_LE+F_SSINGLE:
      if (FloatReg(fs) <= FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_LE+F_SDOUBLE:
      if (DoubleReg(fs) <= DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_LT+F_SSINGLE:
      if (FloatReg(fs) < FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_LT+F_SDOUBLE:
      if (DoubleReg(fs) < DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_NGE+F_SSINGLE:
      if (FloatReg(fs) < FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_NGE+F_SDOUBLE:
      if (DoubleReg(fs) < DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_NGLE+F_SSINGLE:
      P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_NGLE+F_SDOUBLE:
      P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_NGT+F_SSINGLE:
      if (FloatReg(fs) <= FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_NGT+F_SDOUBLE:
      if (DoubleReg(fs) <= DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_OLE+F_SSINGLE:
      if (FloatReg(fs) < FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_OLE+F_SDOUBLE:
      if (DoubleReg(fs) < DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_OLT+F_SSINGLE:
      if (FloatReg(fs) < FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_OLT+F_SDOUBLE:

     if (DoubleReg(fs) < DoubleReg(ft)) {
       P->FCR[31] |= M_csr_c;
     } else {
       P->FCR[31] &= ~M_csr_c;
     }
     break;

   case F_C_SEQ+F_SSINGLE:
      if (FloatReg(fs) == FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_SEQ+F_SDOUBLE:
      if (DoubleReg(fs) == DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_SF+F_SSINGLE:
      P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_SF+F_SDOUBLE:
      P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_UEQ+F_SSINGLE:
      if (FloatReg(fs) == FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_UEQ+F_SDOUBLE:
      if (DoubleReg(fs) == DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_ULE+F_SSINGLE:
      if (FloatReg(fs) <= FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_ULE+F_SDOUBLE:
      if (DoubleReg(fs) <= DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_ULT+F_SSINGLE:
      if (FloatReg(fs) < FloatReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_ULT+F_SDOUBLE:
      if (DoubleReg(fs) < DoubleReg(ft))
         P->FCR[31] |= M_csr_c;
      else
         P->FCR[31] &= ~M_csr_c;
      break;

   case F_C_UN+F_SSINGLE:
      P->FCR[31] &= ~M_csr_c;
      break;
   case F_C_UN+F_SDOUBLE:
      P->FCR[31] &= ~M_csr_c;
      break;

   default:
      CPUWarning("ERROR!!! FPU instruction 0x%x unimplemented at PC 0x%llx\n",
		 instr, (Reg64)P->PC);
      ASSERT(0);
   } /* cp1_function */

   return C1_SUCCESS;
}

/*****************************************************************
 * MipsyFPU
 *****************************************************************/
static Result 
MipsyFPUx(CPUState *P, Inst instr) 
{
   uint ft  = FT(instr); /* 2nd source register */
   uint fs  = FS(instr); /* 1st source reg */
   uint fr  = FORMAT(instr); /* 3rd source reg. */
   uint fd  = FD(instr); /* destionation */

   uint func = FUNC(instr);

   switch(func) {
   case madd_s_op:
      FloatReg(fd) = (FloatReg(fs) * FloatReg(ft)) + FloatReg(fr);
      break;
   case madd_d_op:
      DoubleReg(fd) = (DoubleReg(fs) * DoubleReg(ft)) + DoubleReg(fr);
      break;
   case msub_s_op:
      FloatReg(fd) = (FloatReg(fs) * FloatReg(ft)) - FloatReg(fr);
      break;
   case msub_d_op:
      DoubleReg(fd) = (DoubleReg(fs) * DoubleReg(ft)) - DoubleReg(fr);
      break;
   case nmadd_s_op:
      FloatReg(fd) = -((FloatReg(fs) * FloatReg(ft)) + FloatReg(fr));
      break;
   case nmadd_d_op:
      DoubleReg(fd) = -((DoubleReg(fs) * DoubleReg(ft)) + DoubleReg(fr));
      break;
   case nmsub_s_op:
      FloatReg(fd) = -((FloatReg(fs) * FloatReg(ft)) - FloatReg(fr));
      break;
   case nmsub_d_op:
      DoubleReg(fd) = -((DoubleReg(fs) * DoubleReg(ft)) - DoubleReg(fr));
      break;
   default:
      EXCEPTION(P, EXC_II);
      return C1_CONTINUE;
   }
   return C1_SUCCESS;
}

/*****************************************************************
 * Set the rounding mode of the host processor to match that of the
 * simulated machine. This is needed since we're actually executing the
 * instructions on the host platform. 
 *****************************************************************/
static void 
SetRoundingMode(int modeNum)
{
#if defined(sgi) || defined(sparc)
   fp_rnd newMode = FP_RN; 
   
   switch (modeNum) {
   case 0:
      newMode = FP_RN;
      break;
   case 1:
      newMode = FP_RZ;
      break;
   case 2:
      newMode = FP_RP;
      break;
   case 3:
      newMode = FP_RM;
      break;
   default:
      CPUWarning("Bad rounding mode set in FCR31 (%d)\n", modeNum);
      break;
   }
   fpsetround(newMode);
#endif
#ifdef __alpha
   write_rnd(modeNum);
#endif
#ifdef linux
   {
      static int fpwarned = 0;
      if (!fpwarned) {
         CPUWarning("MIPSY: could not figure out how to fpsetround on x86 \n");
         fpwarned = 1;
      }
   }
#endif
}

/*****************************************************************
 * All fpu (COP1) insts come through here.
 *****************************************************************/
int 
ExecuteC1Instruction(CPUState *P, Inst instr)
{
   int immediate   = IMMED(instr);
   uint fs  = FS(instr); /* 1st source reg */
   uint ft  = FT(instr); /* 2nd source register */
   uint sub_value = FORMAT(instr);
   uint br_value  = ft;
   uint major_op;
   Result result = C1_SUCCESS;
   uint func = FUNC(instr);
   StatusReg statusReg;
   
   /* The kernel sets the cop1 usable flag to 0 when it */
   /* first starts a new context to limit the saving of */
   /* FPU registers to those procs who use them. The */
   /* first time a process uses a FPU instruction it */
   /* needs to trap into the kernel for it to set this */
   /* bit. */
   
   statusReg.ts_data = P->CP0[C0_SR];

   if (!(statusReg.s32.ts_cu1)) {
      CauseReg  causeReg;
      
      causeReg.tc_data = P->CP0[C0_CAUSE];
      causeReg.s32.tc_ce = 1;
      P->CP0[C0_CAUSE] = causeReg.tc_data;
      
      EXCEPTION(P, EXC_CPU); 
      return C1_CONTINUE;
   }

   SetRoundingMode(P->FCR[31] & 0x3);

   major_op = MAJOR_OP(instr);
   if (major_op == cop1_op) { 

      switch(sub_value) {

      case bc_op:
         if (br_value & 0x1) { /* BC1T */
            /* Check if the condition bit in the csr is set */
            if (P->FCR[31] & M_csr_c) {
               P->branchTarget = P->nPC + SIGN_EXTEND(18, (immediate << 2)); 
               P->branchStatus = BranchStatus_taken;
            }  else { 
		if( br_value & 0x2 ) { /* branch likely */
		    /* Skip instruction in delay slot */
		    P->nPC = P->nPC + 4;
		} else {
		    P->branchStatus = BranchStatus_nottaken;
                } 
            }
         } else { /* BC1F */
            /* Check if condition bit not set */
            if (!(P->FCR[31] & M_csr_c)) {
               P->branchTarget = P->nPC + SIGN_EXTEND(18, (immediate << 2));
               P->branchStatus = BranchStatus_taken;
            }  else { 
		if( br_value & 0x2 ) { /* branch likely */
		    /* Skip instruction in delay slot */
		    P->nPC = P->nPC + 4;
		} else {
		    P->branchStatus = BranchStatus_nottaken;
                }
            }
         }
         break;
      
      case cfc_op:
         if (fs == 31) {
            P->R[ft] = P->FCR[fs];
         } else {
            P->R[ft] = (3 << 8); /* R3010 FPU */
         }
         SIM_DEBUG(('f', "C1: CFC1 got %#x from fcr %d\n", P->R[ft], fs));
         break;
      
      case ctc_op:
         P->FCR[fs] = P->R[ft];
         SIM_DEBUG(('f', "C1: CTC1 put %#x into fcr %d\n", P->R[ft], fs));
         break;
      
      case mfc_op: 
         P->R[ft] = (Reg32_s)IntReg(fs);
         SIM_DEBUG(('f', "C1: mfc1 put f%d %d into r%d \n", fs, IntReg(fs), ft));
         break;
      
      case mtc_op:
         IntReg(fs) = (Reg32)P->R[ft];
         SIM_DEBUG(('f', "C1: mtc1 put f%d %d into f%d -> %d\n", ft, P->R[ft], 
               fs, IntReg(fs)));
         break;
  
      case dmfc_op: 
         CHECK_64BIT_ALLOWED(P)
            P->R[ft] = LongLongReg(fs);
         SIM_DEBUG(('f', "C1: dmfc1 put f%d %lld into r%d \n", fs, LongLongReg(fs), ft));
         break;
      
      case dmtc_op:
         CHECK_64BIT_ALLOWED(P)
            LongLongReg(fs) = P->R[ft];
         SIM_DEBUG(('f', "C1: dmtc1 put f%d %lldd into f%d -> %lld\n", ft, P->R[ft], 
               fs, LongLongReg(fs)));
         break;
      default: 
         /* S, D, W format instructions */
         switch (func) {
         case fmovc_op:
         case fmovn_op:
         case fmovz_op: { 
            /* Make sure we can execute mips4 instructions */
            if (IS_USER_MODE(P) && !(statusReg.s32.ts_xx)) {
               EXCEPTION(P, EXC_II); 
               return C1_CONTINUE;
            }
            break;
         }
         default:
            STATS_INC(P->myNum, numFPOps, 1);
            break;
         } 
         result = MipsyFPU(P, instr);
         break;
      } /* switch subvalue */
   } else if (major_op == cop1x_op) {
      if (IS_USER_MODE(P) && !(statusReg.s32.ts_xx)) {
         EXCEPTION(P, EXC_II); 
         return C1_CONTINUE;
      }
      STATS_INC(P->myNum, numFPOps, 1);
      result = MipsyFPUx(P,instr);
      
   } else if (major_op == spec_op) { 
      /* Must be on the MOVC instructions */
      ASSERT(FUNC(instr) == movc_op);
      /* Move conditional on FP condition */
      if (TF(instr)) { 
         if (P->FCR[31] & M_csr_c) {
            P->R[RT(instr)] = P->R[RS(instr)];
         }
      } else {
         if (!(P->FCR[31] & M_csr_c)) {
            P->R[RT(instr)] = P->R[RS(instr)];
         }
      }
   } else {
      ASSERT(0); /* unknown C1 instruction */
   }
      
   return result;
}