1// See LICENSE for license details.
2
3#include "emulation.h"
4#include "fp_emulation.h"
5#include "unprivileged_memory.h"
6#include "mtrap.h"
7#include "config.h"
8#include "pk.h"
9
10union byte_array {
11  uint8_t bytes[8];
12  uintptr_t intx;
13  uint64_t int64;
14};
15
16void misaligned_load_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
17{
18  union byte_array val;
19  uintptr_t mstatus;
20  insn_t insn = get_insn(mepc, &mstatus);
21  uintptr_t npc = mepc + insn_len(insn);
22  uintptr_t addr = read_csr(mbadaddr);
23
24  int shift = 0, fp = 0, len;
25  if ((insn & MASK_LW) == MATCH_LW)
26    len = 4, shift = 8*(sizeof(uintptr_t) - len);
27#if __riscv_xlen == 64
28  else if ((insn & MASK_LD) == MATCH_LD)
29    len = 8, shift = 8*(sizeof(uintptr_t) - len);
30  else if ((insn & MASK_LWU) == MATCH_LWU)
31    len = 4;
32#endif
33#ifdef PK_ENABLE_FP_EMULATION
34  else if ((insn & MASK_FLD) == MATCH_FLD)
35    fp = 1, len = 8;
36  else if ((insn & MASK_FLW) == MATCH_FLW)
37    fp = 1, len = 4;
38#endif
39  else if ((insn & MASK_LH) == MATCH_LH)
40    len = 2, shift = 8*(sizeof(uintptr_t) - len);
41  else if ((insn & MASK_LHU) == MATCH_LHU)
42    len = 2;
43#ifdef __riscv_compressed
44# if __riscv_xlen >= 64
45  else if ((insn & MASK_C_LD) == MATCH_C_LD)
46    len = 8, shift = 8*(sizeof(uintptr_t) - len), insn = RVC_RS2S(insn) << SH_RD;
47  else if ((insn & MASK_C_LDSP) == MATCH_C_LDSP && ((insn >> SH_RD) & 0x1f))
48    len = 8, shift = 8*(sizeof(uintptr_t) - len);
49# endif
50  else if ((insn & MASK_C_LW) == MATCH_C_LW)
51    len = 4, shift = 8*(sizeof(uintptr_t) - len), insn = RVC_RS2S(insn) << SH_RD;
52  else if ((insn & MASK_C_LWSP) == MATCH_C_LWSP && ((insn >> SH_RD) & 0x1f))
53    len = 4, shift = 8*(sizeof(uintptr_t) - len);
54# ifdef PK_ENABLE_FP_EMULATION
55  else if ((insn & MASK_C_FLD) == MATCH_C_FLD)
56    fp = 1, len = 8, insn = RVC_RS2S(insn) << SH_RD;
57  else if ((insn & MASK_C_FLDSP) == MATCH_C_FLDSP)
58    fp = 1, len = 8;
59#  if __riscv_xlen == 32
60  else if ((insn & MASK_C_FLW) == MATCH_C_FLW)
61    fp = 1, len = 4, insn = RVC_RS2S(insn) << SH_RD;
62  else if ((insn & MASK_C_FLWSP) == MATCH_C_FLWSP)
63    fp = 1, len = 4;
64#  endif
65# endif
66#endif
67  else {
68    mcause = CAUSE_LOAD_ACCESS;
69    write_csr(mcause, mcause);
70    return truly_illegal_insn(regs, mcause, mepc, mstatus, insn);
71  }
72
73  val.int64 = 0;
74  for (intptr_t i = 0; i < len; i++)
75    val.bytes[i] = load_uint8_t((void *)(addr + i), mepc);
76
77  if (!fp)
78    SET_RD(insn, regs, (intptr_t)val.intx << shift >> shift);
79  else if (len == 8)
80    SET_F64_RD(insn, regs, val.int64);
81  else
82    SET_F32_RD(insn, regs, val.intx);
83
84  write_csr(mepc, npc);
85}
86
87void misaligned_store_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
88{
89  union byte_array val;
90  uintptr_t mstatus;
91  insn_t insn = get_insn(mepc, &mstatus);
92  uintptr_t npc = mepc + insn_len(insn);
93  int len;
94
95  val.intx = GET_RS2(insn, regs);
96  if ((insn & MASK_SW) == MATCH_SW)
97    len = 4;
98#if __riscv_xlen == 64
99  else if ((insn & MASK_SD) == MATCH_SD)
100    len = 8;
101#endif
102#ifdef PK_ENABLE_FP_EMULATION
103  else if ((insn & MASK_FSD) == MATCH_FSD)
104    len = 8, val.int64 = GET_F64_RS2(insn, regs);
105  else if ((insn & MASK_FSW) == MATCH_FSW)
106    len = 4, val.intx = GET_F32_RS2(insn, regs);
107#endif
108  else if ((insn & MASK_SH) == MATCH_SH)
109    len = 2;
110#ifdef __riscv_compressed
111# if __riscv_xlen >= 64
112  else if ((insn & MASK_C_SD) == MATCH_C_SD)
113    len = 8, val.intx = GET_RS2S(insn, regs);
114  else if ((insn & MASK_C_SDSP) == MATCH_C_SDSP && ((insn >> SH_RD) & 0x1f))
115    len = 8, val.intx = GET_RS2C(insn, regs);
116# endif
117  else if ((insn & MASK_C_SW) == MATCH_C_SW)
118    len = 4, val.intx = GET_RS2S(insn, regs);
119  else if ((insn & MASK_C_SWSP) == MATCH_C_SWSP && ((insn >> SH_RD) & 0x1f))
120    len = 4, val.intx = GET_RS2C(insn, regs);
121# ifdef PK_ENABLE_FP_EMULATION
122  else if ((insn & MASK_C_FSD) == MATCH_C_FSD)
123    len = 8, val.int64 = GET_F64_RS2S(insn, regs);
124  else if ((insn & MASK_C_FSDSP) == MATCH_C_FSDSP)
125    len = 8, val.int64 = GET_F64_RS2C(insn, regs);
126#  if __riscv_xlen == 32
127  else if ((insn & MASK_C_FSW) == MATCH_C_FSW)
128    len = 4, val.intx = GET_F32_RS2S(insn, regs);
129  else if ((insn & MASK_C_FSWSP) == MATCH_C_FSWSP)
130    len = 4, val.intx = GET_F32_RS2C(insn, regs);
131#  endif
132# endif
133#endif
134  else {
135    mcause = CAUSE_STORE_ACCESS;
136    write_csr(mcause, mcause);
137    return truly_illegal_insn(regs, mcause, mepc, mstatus, insn);
138  }
139
140  uintptr_t addr = read_csr(mbadaddr);
141  for (int i = 0; i < len; i++)
142    store_uint8_t((void *)(addr + i), val.bytes[i], mepc);
143
144  write_csr(mepc, npc);
145}
146