Patrick Georgi | d1e50f9 | 2020-03-04 15:00:05 +0100 | [diff] [blame] | 1 | /* SPDX-License-Identifier: GPL-2.0-only */ |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 2 | |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 3 | #include <vm.h> |
| 4 | #include <arch/exception.h> |
| 5 | #include <commonlib/helpers.h> |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 6 | #include <types.h> |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 7 | |
| 8 | /* these functions are defined in src/arch/riscv/fp_asm.S */ |
| 9 | #if defined(__riscv_flen) |
| 10 | #if __riscv_flen >= 32 |
| 11 | extern void read_f32(int regnum, uint32_t *v); |
| 12 | extern void write_f32(int regnum, uint32_t *v); |
| 13 | #endif // __riscv_flen >= 32 |
| 14 | #if __riscv_flen >= 64 |
| 15 | extern void read_f64(int regnum, uint64_t *v); |
| 16 | extern void write_f64(int regnum, uint64_t *v); |
| 17 | #endif // __riscv_flen >= 64 |
| 18 | #endif // defined(__riscv_flen) |
| 19 | |
| 20 | /* This union makes it easy to read multibyte types by byte operations. */ |
| 21 | union endian_buf { |
| 22 | uint8_t b[8]; |
| 23 | uint16_t h[4]; |
| 24 | uint32_t w[2]; |
| 25 | uint64_t d[1]; |
| 26 | uintptr_t v; |
| 27 | }; |
| 28 | |
| 29 | /* This struct hold info of load/store instruction */ |
| 30 | struct memory_instruction_info { |
| 31 | /* opcode/mask used to identify instruction, |
| 32 | * (instruction_val) & mask == opcode */ |
| 33 | uint32_t opcode; |
| 34 | uint32_t mask; |
| 35 | /* reg_shift/reg_mask/reg_addition used to get register number |
| 36 | * ((instruction_val >> reg_shift) & reg_mask) + reg_addition */ |
| 37 | unsigned int reg_shift; |
| 38 | unsigned int reg_mask; |
| 39 | unsigned int reg_addition; |
| 40 | unsigned int is_fp : 1; /* mark as a float operation */ |
| 41 | unsigned int is_load : 1; /* mark as a load operation */ |
| 42 | unsigned int width : 8; /* Record the memory width of the operation */ |
| 43 | unsigned int sign_extend : 1; /* mark need to be sign extended */ |
| 44 | }; |
| 45 | |
| 46 | static struct memory_instruction_info insn_info[] = { |
| 47 | #if __riscv_xlen == 128 |
| 48 | { 0x00002000, 0x0000e003, 2, 7, 8, 0, 1, 16, 1}, // C.LQ |
| 49 | #else |
| 50 | { 0x00002000, 0x0000e003, 2, 7, 8, 1, 1, 8, 0}, // C.FLD |
| 51 | #endif |
| 52 | { 0x00004000, 0x0000e003, 2, 7, 8, 0, 1, 4, 1}, // C.LW |
| 53 | #if __riscv_xlen == 32 |
| 54 | { 0x00006000, 0x0000e003, 2, 7, 8, 1, 1, 4, 0}, // C.FLW |
| 55 | #else |
| 56 | { 0x00006000, 0x0000e003, 2, 7, 8, 0, 1, 8, 1}, // C.LD |
| 57 | #endif |
| 58 | |
| 59 | #if __riscv_xlen == 128 |
| 60 | { 0x0000a000, 0x0000e003, 2, 7, 8, 0, 0, 16, 0}, // C.SQ |
| 61 | #else |
| 62 | { 0x0000a000, 0x0000e003, 2, 7, 8, 1, 0, 8, 0}, // C.FSD |
| 63 | #endif |
| 64 | { 0x0000c000, 0x0000e003, 2, 7, 8, 0, 0, 4, 0}, // C.SW |
| 65 | #if __riscv_xlen == 32 |
| 66 | { 0x0000e000, 0x0000e003, 2, 7, 8, 1, 0, 4, 0}, // C.FSW |
| 67 | #else |
| 68 | { 0x0000e000, 0x0000e003, 2, 7, 8, 0, 0, 8, 0}, // C.SD |
| 69 | #endif |
| 70 | |
| 71 | #if __riscv_xlen == 128 |
| 72 | { 0x00002002, 0x0000e003, 7, 15, 0, 0, 1, 16, 1}, // C.LQSP |
| 73 | #else |
| 74 | { 0x00002002, 0x0000e003, 7, 15, 0, 1, 1, 8, 0}, // C.FLDSP |
| 75 | #endif |
| 76 | { 0x00004002, 0x0000e003, 7, 15, 0, 0, 1, 4, 1}, // C.LWSP |
| 77 | #if __riscv_xlen == 32 |
| 78 | { 0x00006002, 0x0000e003, 7, 15, 0, 1, 1, 4, 0}, // C.FLWSP |
| 79 | #else |
| 80 | { 0x00006002, 0x0000e003, 7, 15, 0, 0, 1, 8, 1}, // C.LDSP |
| 81 | #endif |
| 82 | |
| 83 | #if __riscv_xlen == 128 |
| 84 | { 0x0000a002, 0x0000e003, 2, 15, 0, 0, 0, 16, 0}, // C.SQSP |
| 85 | #else |
| 86 | { 0x0000a002, 0x0000e003, 2, 15, 0, 1, 0, 8, 0}, // C.FSDSP |
| 87 | #endif |
| 88 | { 0x0000c002, 0x0000e003, 2, 15, 0, 0, 0, 4, 0}, // C.SWSP |
| 89 | #if __riscv_xlen == 32 |
| 90 | { 0x0000e002, 0x0000e003, 2, 15, 0, 1, 0, 4, 0}, // C.FSWSP |
| 91 | #else |
| 92 | { 0x0000e002, 0x0000e003, 2, 15, 0, 0, 0, 8, 0}, // C.SDSP |
| 93 | #endif |
| 94 | |
| 95 | { 0x00000003, 0x0000707f, 7, 15, 0, 0, 1, 1, 1}, // LB |
| 96 | { 0x00001003, 0x0000707f, 7, 15, 0, 0, 1, 2, 1}, // LH |
| 97 | { 0x00002003, 0x0000707f, 7, 15, 0, 0, 1, 4, 1}, // LW |
| 98 | #if __riscv_xlen > 32 |
| 99 | { 0x00003003, 0x0000707f, 7, 15, 0, 0, 1, 8, 1}, // LD |
| 100 | #endif |
| 101 | { 0x00004003, 0x0000707f, 7, 15, 0, 0, 1, 1, 0}, // LBU |
| 102 | { 0x00005003, 0x0000707f, 7, 15, 0, 0, 1, 2, 0}, // LHU |
| 103 | { 0x00006003, 0x0000707f, 7, 15, 0, 0, 1, 4, 0}, // LWU |
| 104 | |
| 105 | { 0x00000023, 0x0000707f, 20, 15, 0, 0, 0, 1, 0}, // SB |
| 106 | { 0x00001023, 0x0000707f, 20, 15, 0, 0, 0, 2, 0}, // SH |
| 107 | { 0x00002023, 0x0000707f, 20, 15, 0, 0, 0, 4, 0}, // SW |
| 108 | #if __riscv_xlen > 32 |
| 109 | { 0x00003023, 0x0000707f, 20, 15, 0, 0, 0, 8, 0}, // SD |
| 110 | #endif |
| 111 | |
| 112 | #if defined(__riscv_flen) |
| 113 | #if __riscv_flen >= 32 |
| 114 | { 0x00002007, 0x0000707f, 7, 15, 0, 1, 1, 4, 0}, // FLW |
| 115 | { 0x00003007, 0x0000707f, 7, 15, 0, 1, 1, 8, 0}, // FLD |
| 116 | #endif // __riscv_flen >= 32 |
| 117 | |
| 118 | #if __riscv_flen >= 64 |
| 119 | { 0x00002027, 0x0000707f, 20, 15, 0, 1, 0, 4, 0}, // FSW |
| 120 | { 0x00003027, 0x0000707f, 20, 15, 0, 1, 0, 8, 0}, // FSD |
| 121 | #endif // __riscv_flen >= 64 |
| 122 | #endif // defined(__riscv_flen) |
| 123 | }; |
| 124 | |
| 125 | static struct memory_instruction_info *match_instruction(uintptr_t insn) |
| 126 | { |
| 127 | int i; |
| 128 | for (i = 0; i < ARRAY_SIZE(insn_info); i++) |
| 129 | if ((insn_info[i].mask & insn) == insn_info[i].opcode) |
| 130 | return &(insn_info[i]); |
| 131 | return NULL; |
| 132 | } |
| 133 | |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 134 | static enum cb_err fetch_16bit_instruction(uintptr_t vaddr, uintptr_t *insn, int *size) |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 135 | { |
| 136 | uint16_t ins = mprv_read_mxr_u16((uint16_t *)vaddr); |
| 137 | if (EXTRACT_FIELD(ins, 0x3) != 3) { |
| 138 | *insn = ins; |
Jonathan Neuschäfer | ae91cda | 2018-09-21 18:57:04 +0200 | [diff] [blame] | 139 | *size = 2; |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 140 | return CB_SUCCESS; |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 141 | } |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 142 | return CB_ERR; |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 143 | } |
| 144 | |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 145 | static enum cb_err fetch_32bit_instruction(uintptr_t vaddr, uintptr_t *insn, int *size) |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 146 | { |
| 147 | uint32_t l = (uint32_t)mprv_read_mxr_u16((uint16_t *)vaddr + 0); |
Philipp Hug | d4ab5bb | 2018-10-29 17:55:55 +0100 | [diff] [blame] | 148 | uint32_t h = (uint32_t)mprv_read_mxr_u16((uint16_t *)vaddr + 1); |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 149 | uint32_t ins = (h << 16) | l; |
| 150 | if ((EXTRACT_FIELD(ins, 0x3) == 3) && |
| 151 | (EXTRACT_FIELD(ins, 0x1c) != 0x7)) { |
| 152 | *insn = ins; |
Jonathan Neuschäfer | ae91cda | 2018-09-21 18:57:04 +0200 | [diff] [blame] | 153 | *size = 4; |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 154 | return CB_SUCCESS; |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 155 | } |
Elyes Haouas | 2179c7f | 2022-10-13 12:07:24 +0200 | [diff] [blame^] | 156 | return CB_ERR; |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 157 | } |
| 158 | |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 159 | void handle_misaligned(trapframe *tf) |
| 160 | { |
| 161 | uintptr_t insn = 0; |
| 162 | union endian_buf buff; |
Jonathan Neuschäfer | ae91cda | 2018-09-21 18:57:04 +0200 | [diff] [blame] | 163 | int insn_size = 0; |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 164 | |
| 165 | /* try to fetch 16/32 bits instruction */ |
Jonathan Neuschäfer | ae91cda | 2018-09-21 18:57:04 +0200 | [diff] [blame] | 166 | if (fetch_16bit_instruction(tf->epc, &insn, &insn_size) < 0) { |
| 167 | if (fetch_32bit_instruction(tf->epc, &insn, &insn_size) < 0) { |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 168 | redirect_trap(); |
Jonathan Neuschäfer | ae91cda | 2018-09-21 18:57:04 +0200 | [diff] [blame] | 169 | return; |
| 170 | } |
| 171 | } |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 172 | |
| 173 | /* matching instruction */ |
| 174 | struct memory_instruction_info *match = match_instruction(insn); |
| 175 | |
| 176 | if (!match) { |
| 177 | redirect_trap(); |
| 178 | return; |
| 179 | } |
| 180 | |
| 181 | int regnum; |
| 182 | regnum = ((insn >> match->reg_shift) & match->reg_mask); |
| 183 | regnum = regnum + match->reg_addition; |
| 184 | buff.v = 0; |
| 185 | if (match->is_load) { |
| 186 | /* load operation */ |
| 187 | |
| 188 | /* reading from memory by bytes prevents misaligned |
| 189 | * memory access */ |
| 190 | for (int i = 0; i < match->width; i++) { |
| 191 | uint8_t *addr = (uint8_t *)(tf->badvaddr + i); |
| 192 | buff.b[i] = mprv_read_u8(addr); |
| 193 | } |
| 194 | |
| 195 | /* sign extend for signed integer loading */ |
| 196 | if (match->sign_extend) |
| 197 | if (buff.v >> (8 * match->width - 1)) |
| 198 | buff.v |= -1 << (8 * match->width); |
| 199 | |
| 200 | /* write to register */ |
| 201 | if (match->is_fp) { |
| 202 | int done = 0; |
| 203 | #if defined(__riscv_flen) |
| 204 | #if __riscv_flen >= 32 |
| 205 | /* single-precision floating-point */ |
| 206 | if (match->width == 4) { |
| 207 | write_f32(regnum, buff.w); |
| 208 | done = 1; |
| 209 | } |
| 210 | #endif // __riscv_flen >= 32 |
| 211 | #if __riscv_flen >= 64 |
| 212 | /* double-precision floating-point */ |
| 213 | if (match->width == 8) { |
| 214 | write_f64(regnum, buff.d); |
| 215 | done = 1; |
| 216 | } |
| 217 | #endif // __riscv_flen >= 64 |
| 218 | #endif // defined(__riscv_flen) |
| 219 | if (!done) |
| 220 | redirect_trap(); |
| 221 | } else { |
| 222 | tf->gpr[regnum] = buff.v; |
| 223 | } |
| 224 | } else { |
| 225 | /* store operation */ |
| 226 | |
| 227 | /* reading from register */ |
| 228 | if (match->is_fp) { |
| 229 | int done = 0; |
| 230 | #if defined(__riscv_flen) |
| 231 | #if __riscv_flen >= 32 |
| 232 | if (match->width == 4) { |
| 233 | read_f32(regnum, buff.w); |
| 234 | done = 1; |
| 235 | } |
| 236 | #endif // __riscv_flen >= 32 |
| 237 | #if __riscv_flen >= 64 |
| 238 | if (match->width == 8) { |
| 239 | read_f64(regnum, buff.d); |
| 240 | done = 1; |
| 241 | } |
| 242 | #endif // __riscv_flen >= 64 |
| 243 | #endif // defined(__riscv_flen) |
| 244 | if (!done) |
| 245 | redirect_trap(); |
| 246 | } else { |
| 247 | buff.v = tf->gpr[regnum]; |
| 248 | } |
| 249 | |
| 250 | /* writing to memory by bytes prevents misaligned |
| 251 | * memory access */ |
| 252 | for (int i = 0; i < match->width; i++) { |
| 253 | uint8_t *addr = (uint8_t *)(tf->badvaddr + i); |
| 254 | mprv_write_u8(addr, buff.b[i]); |
| 255 | } |
| 256 | } |
Jonathan Neuschäfer | ae91cda | 2018-09-21 18:57:04 +0200 | [diff] [blame] | 257 | |
| 258 | /* return to where we came from */ |
| 259 | write_csr(mepc, read_csr(mepc) + insn_size); |
Xiang Wang | cda59b5 | 2018-08-09 16:20:35 +0800 | [diff] [blame] | 260 | } |