kern/arch/riscv/fpu.c

   1 #include <trap.h>
   2 #include <smp.h>
   3 #include <umem.h>
   4 #include <arch/softfloat.h>
   5
   6 static uint32_t ls(uint64_t* addr)
   7 {
   8         uint32_t r;
   9         asm ("fld f0, %1; mftx.s %0, f0" : "=r"(r) : "m"(*addr));
  10         return r;
  11 }
  12
  13 static void ss(uint64_t* addr, uint32_t val)
  14 {
  15         asm ("mxtf.s f0, %0; fsd f0, %1" : : "r"(val), "m"(*addr));
  16 }
  17
  18 static int emulate_fpu_silly(struct hw_trapframe *state,
  19                              ancillary_state_t *silly)
  20 {
  21         int insn;
  22         if (memcpy_from_user(current, &insn, (void*)state->epc, 4))
  23         {
  24                 state->cause = CAUSE_FAULT_FETCH;
  25                 handle_trap(state);
  26         }
  27
  28         #define DECLARE_INSN(name, match, mask) bool is_##name = (insn & mask) == match;
  29         #include <arch/opcodes.h>
  30         #undef DECLARE_INSN
  31
  32         int rd  = (insn >> 27) & 0x1f;
  33         int rs1 = (insn >> 22) & 0x1f;
  34         int rs2 = (insn >> 17) & 0x1f;
  35         int rs3 = (insn >> 12) & 0x1f;
  36
  37         int imm = (insn << 10) >> 20;
  38         int bimm = ((insn >> 10) & 0x7f) | ((insn & 0xf8000000) >> 20);
  39
  40         void* load_address = (void*)(state->gpr[rs1] + imm);
  41         void* store_address = (void*)(state->gpr[rs1] + bimm);
  42
  43         softfloat_t sf;
  44         sf.float_rounding_mode = silly->fsr >> 5;
  45         sf.float_exception_flags = silly->fsr & 0x1f;
  46
  47         if (is_fsqrt_s)
  48                 ss(&silly->fpr[rd], float32_sqrt(&sf, ls(&silly->fpr[rs1])));
  49         else if (is_fsqrt_d)
  50                 silly->fpr[rd] = float64_sqrt(&sf, silly->fpr[rs1]);
  51         else if (is_fdiv_s)
  52                 ss(&silly->fpr[rd], float32_div(&sf, ls(&silly->fpr[rs1]), ls(&silly->fpr[rs2])));
  53         else if (is_fdiv_d)
  54                 silly->fpr[rd] = float64_div(&sf, silly->fpr[rs1], silly->fpr[rs2]);
  55         /* Eventually, we will emulate the full FPU, including the below insns
  56         else if (is_mffsr)
  57         {
  58                 // use sf instead of silly->fsr
  59                 state->gpr[rd] = silly->fsr;
  60         }
  61         else if (is_mtfsr)
  62         {
  63                 // use sf instead of silly->fsr
  64                 int temp = silly->fsr;
  65                 silly->fsr = state->gpr[rs1] & 0xFF;
  66                 state->gpr[rd] = silly->fsr;
  67         }
  68         else if (is_fld)
  69         {
  70                 uint64_t dest;
  71                 if (!memcpy_from_user(current, &dest, load_address, sizeof(dest)))
  72                 {
  73                         state->cause = CAUSE_FAULT_LOAD;
  74                         state->badvaddr = (long)load_address;
  75                         handle_trap(state);
  76                 }
  77                 silly->fpr[rd] = dest;
  78         }
  79         else if (is_flw)
  80         {
  81                 uint32_t dest;
  82                 if (!memcpy_from_user(current, &dest, load_address, sizeof(dest)))
  83                 {
  84                         state->cause = CAUSE_FAULT_LOAD;
  85                         state->badvaddr = (long)load_address;
  86                         handle_trap(state);
  87                 }
  88                 silly->fpr[rd] = dest;
  89         }
  90         else if (is_fsd)
  91         {
  92                 if (!memcpy_to_user(current, store_address, &silly->fpr[rs2], sizeof(uint64_t)))
  93                 {
  94                         state->cause = CAUSE_FAULT_STORE;
  95                         state->badvaddr = (long)store_address;
  96                         handle_trap(state);
  97                 }
  98         }
  99         else if (is_flw)
 100         {
 101                 if (!memcpy_to_user(current, store_address, &silly->fpr[rs2], sizeof(uint32_t)))
 102                 {
 103                         state->cause = CAUSE_FAULT_STORE;
 104                         state->badvaddr = (long)store_address;
 105                         handle_trap(state);
 106                 }
 107         }
 108         */
 109         else
 110           return 1;
 111
 112         silly->fsr = sf.float_rounding_mode << 5 | sf.float_exception_flags;
 113         return 0;
 114 }
 115
 116 /* For now we can only emulate missing compute insns, not the whole FPU */
 117 int emulate_fpu(struct hw_trapframe *state)
 118 {
 119         if (!(state->sr & SR_EF))
 120         {
 121                 state->cause = CAUSE_FP_DISABLED;
 122                 handle_trap(state);
 123         }
 124
 125         ancillary_state_t fp_state;
 126         save_fp_state(&fp_state);
 127         int code = emulate_fpu_silly(state, &fp_state);
 128         restore_fp_state(&fp_state);
 129         return code;
 130 }