VMM: Clean up per-cpu VMCS state
[akaros.git] / 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 }