risc-v bugfix potpourri
[akaros.git] / kern / arch / riscv / fpu.c
1 #include <arch/trap.h>
2 #include <smp.h>
3 #include <umem.h>
4
5 int emulate_fpu(struct trapframe* state, ancillary_state_t* silly)
6 {
7         int insn;
8         if (!memcpy_from_user(current, &insn, (void*)state->epc, 4))
9         {
10                 state->cause = CAUSE_FAULT_FETCH;
11                 handle_trap(state);
12         }
13
14         #define DECLARE_INSN(name, match, mask) bool is_##name = (insn & match) == mask;
15         #include <arch/opcodes.h>
16         #undef DECLARE_INSN
17
18         int rd  = (insn >> 27) & 0x1f;
19         int rs1 = (insn >> 22) & 0x1f;
20         int rs2 = (insn >> 17) & 0x1f;
21         int rs3 = (insn >> 12) & 0x1f;
22
23         int imm = (insn << 10) >> 20;
24         int bimm = ((insn >> 10) & 0x7f) | ((insn & 0xf8000000) >> 20);
25
26         void* load_address = (void*)(state->gpr[rs1] + imm);
27         void* store_address = (void*)(state->gpr[rs1] + bimm);
28
29         if (is_mffsr)
30         {
31                 state->gpr[rd] = silly->fsr;
32         }
33         else if (is_mtfsr)
34         {
35                 silly->fsr = state->gpr[rs1];
36         }
37         else if (is_fld)
38         {
39                 uint64_t dest;
40                 if (!memcpy_from_user(current, &dest, load_address, sizeof(dest)))
41                 {
42                         state->cause = CAUSE_FAULT_LOAD;
43                         state->badvaddr = (long)load_address;
44                         handle_trap(state);
45                 }
46                 silly->fpr[rd] = dest;
47         }
48         else if (is_flw)
49         {
50                 uint32_t dest;
51                 if (!memcpy_from_user(current, &dest, load_address, sizeof(dest)))
52                 {
53                         state->cause = CAUSE_FAULT_LOAD;
54                         state->badvaddr = (long)load_address;
55                         handle_trap(state);
56                 }
57                 silly->fpr[rd] = dest;
58         }
59         else if (is_fsd)
60         {
61                 if (!memcpy_to_user(current, store_address, &silly->fpr[rs2], sizeof(uint64_t)))
62                 {
63                         state->cause = CAUSE_FAULT_STORE;
64                         state->badvaddr = (long)store_address;
65                         handle_trap(state);
66                 }
67         }
68         else if (is_flw)
69         {
70                 if (!memcpy_to_user(current, store_address, &silly->fpr[rs2], sizeof(uint32_t)))
71                 {
72                         state->cause = CAUSE_FAULT_STORE;
73                         state->badvaddr = (long)store_address;
74                         handle_trap(state);
75                 }
76         }
77         else
78           return 1;
79         
80         return 0;
81 }