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