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