src/arch/riscv/misaligned.c

   1 /* SPDX-License-Identifier: GPL-2.0-only */
   2
   3 #include <stdint.h>
   4 #include <vm.h>
   5 #include <arch/exception.h>
   6 #include <commonlib/helpers.h>
   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
 134 static int fetch_16bit_instruction(uintptr_t vaddr, uintptr_t *insn, int *size)
 135 {
 136         uint16_t ins = mprv_read_mxr_u16((uint16_t *)vaddr);
 137         if (EXTRACT_FIELD(ins, 0x3) != 3) {
 138                 *insn = ins;
 139                 *size = 2;
 140                 return 0;
 141         }
 142         return -1;
 143 }
 144
 145 static int fetch_32bit_instruction(uintptr_t vaddr, uintptr_t *insn, int *size)
 146 {
 147         uint32_t l = (uint32_t)mprv_read_mxr_u16((uint16_t *)vaddr + 0);
 148         uint32_t h = (uint32_t)mprv_read_mxr_u16((uint16_t *)vaddr + 1);
 149         uint32_t ins = (h << 16) | l;
 150         if ((EXTRACT_FIELD(ins, 0x3) == 3) &&
 151                 (EXTRACT_FIELD(ins, 0x1c) != 0x7)) {
 152                 *insn = ins;
 153                 *size = 4;
 154                 return 0;
 155         }
 156         return -1;
 157 }
 158
 159 void handle_misaligned(trapframe *tf)
 160 {
 161         uintptr_t insn = 0;
 162         union endian_buf buff;
 163         int insn_size = 0;
 164
 165         /* try to fetch 16/32 bits instruction */
 166         if (fetch_16bit_instruction(tf->epc, &insn, &insn_size) < 0) {
 167                 if (fetch_32bit_instruction(tf->epc, &insn, &insn_size) < 0) {
 168                         redirect_trap();
 169                         return;
 170                 }
 171         }
 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         }
 257
 258         /* return to where we came from */
 259         write_csr(mepc, read_csr(mepc) + insn_size);
 260 }