riscv/kernel/traps_misaligned.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2020 Western Digital Corporation or its affiliates.
   4  */
   5 #include <linux/kernel.h>
   6 #include <linux/init.h>
   7 #include <linux/mm.h>
   8 #include <linux/module.h>
   9 #include <linux/irq.h>
  10 #include <linux/stringify.h>
  11
  12 #include <asm/processor.h>
  13 #include <asm/ptrace.h>
  14 #include <asm/csr.h>
  15
  16 #define INSN_MATCH_LB                   0x3
  17 #define INSN_MASK_LB                    0x707f
  18 #define INSN_MATCH_LH                   0x1003
  19 #define INSN_MASK_LH                    0x707f
  20 #define INSN_MATCH_LW                   0x2003
  21 #define INSN_MASK_LW                    0x707f
  22 #define INSN_MATCH_LD                   0x3003
  23 #define INSN_MASK_LD                    0x707f
  24 #define INSN_MATCH_LBU                  0x4003
  25 #define INSN_MASK_LBU                   0x707f
  26 #define INSN_MATCH_LHU                  0x5003
  27 #define INSN_MASK_LHU                   0x707f
  28 #define INSN_MATCH_LWU                  0x6003
  29 #define INSN_MASK_LWU                   0x707f
  30 #define INSN_MATCH_SB                   0x23
  31 #define INSN_MASK_SB                    0x707f
  32 #define INSN_MATCH_SH                   0x1023
  33 #define INSN_MASK_SH                    0x707f
  34 #define INSN_MATCH_SW                   0x2023
  35 #define INSN_MASK_SW                    0x707f
  36 #define INSN_MATCH_SD                   0x3023
  37 #define INSN_MASK_SD                    0x707f
  38
  39 #define INSN_MATCH_FLW                  0x2007
  40 #define INSN_MASK_FLW                   0x707f
  41 #define INSN_MATCH_FLD                  0x3007
  42 #define INSN_MASK_FLD                   0x707f
  43 #define INSN_MATCH_FLQ                  0x4007
  44 #define INSN_MASK_FLQ                   0x707f
  45 #define INSN_MATCH_FSW                  0x2027
  46 #define INSN_MASK_FSW                   0x707f
  47 #define INSN_MATCH_FSD                  0x3027
  48 #define INSN_MASK_FSD                   0x707f
  49 #define INSN_MATCH_FSQ                  0x4027
  50 #define INSN_MASK_FSQ                   0x707f
  51
  52 #define INSN_MATCH_C_LD                 0x6000
  53 #define INSN_MASK_C_LD                  0xe003
  54 #define INSN_MATCH_C_SD                 0xe000
  55 #define INSN_MASK_C_SD                  0xe003
  56 #define INSN_MATCH_C_LW                 0x4000
  57 #define INSN_MASK_C_LW                  0xe003
  58 #define INSN_MATCH_C_SW                 0xc000
  59 #define INSN_MASK_C_SW                  0xe003
  60 #define INSN_MATCH_C_LDSP               0x6002
  61 #define INSN_MASK_C_LDSP                0xe003
  62 #define INSN_MATCH_C_SDSP               0xe002
  63 #define INSN_MASK_C_SDSP                0xe003
  64 #define INSN_MATCH_C_LWSP               0x4002
  65 #define INSN_MASK_C_LWSP                0xe003
  66 #define INSN_MATCH_C_SWSP               0xc002
  67 #define INSN_MASK_C_SWSP                0xe003
  68
  69 #define INSN_MATCH_C_FLD                0x2000
  70 #define INSN_MASK_C_FLD                 0xe003
  71 #define INSN_MATCH_C_FLW                0x6000
  72 #define INSN_MASK_C_FLW                 0xe003
  73 #define INSN_MATCH_C_FSD                0xa000
  74 #define INSN_MASK_C_FSD                 0xe003
  75 #define INSN_MATCH_C_FSW                0xe000
  76 #define INSN_MASK_C_FSW                 0xe003
  77 #define INSN_MATCH_C_FLDSP              0x2002
  78 #define INSN_MASK_C_FLDSP               0xe003
  79 #define INSN_MATCH_C_FSDSP              0xa002
  80 #define INSN_MASK_C_FSDSP               0xe003
  81 #define INSN_MATCH_C_FLWSP              0x6002
  82 #define INSN_MASK_C_FLWSP               0xe003
  83 #define INSN_MATCH_C_FSWSP              0xe002
  84 #define INSN_MASK_C_FSWSP               0xe003
  85
  86 #define INSN_LEN(insn)                  ((((insn) & 0x3) < 0x3) ? 2 : 4)
  87
  88 #if defined(CONFIG_64BIT)
  89 #define LOG_REGBYTES                    3
  90 #define XLEN                            64
  91 #else
  92 #define LOG_REGBYTES                    2
  93 #define XLEN                            32
  94 #endif
  95 #define REGBYTES                        (1 << LOG_REGBYTES)
  96 #define XLEN_MINUS_16                   ((XLEN) - 16)
  97
  98 #define SH_RD                           7
  99 #define SH_RS1                          15
 100 #define SH_RS2                          20
 101 #define SH_RS2C                         2
 102
 103 #define RV_X(x, s, n)                   (((x) >> (s)) & ((1 << (n)) - 1))
 104 #define RVC_LW_IMM(x)                   ((RV_X(x, 6, 1) << 2) | \
 105                                          (RV_X(x, 10, 3) << 3) | \
 106                                          (RV_X(x, 5, 1) << 6))
 107 #define RVC_LD_IMM(x)                   ((RV_X(x, 10, 3) << 3) | \
 108                                          (RV_X(x, 5, 2) << 6))
 109 #define RVC_LWSP_IMM(x)                 ((RV_X(x, 4, 3) << 2) | \
 110                                          (RV_X(x, 12, 1) << 5) | \
 111                                          (RV_X(x, 2, 2) << 6))
 112 #define RVC_LDSP_IMM(x)                 ((RV_X(x, 5, 2) << 3) | \
 113                                          (RV_X(x, 12, 1) << 5) | \
 114                                          (RV_X(x, 2, 3) << 6))
 115 #define RVC_SWSP_IMM(x)                 ((RV_X(x, 9, 4) << 2) | \
 116                                          (RV_X(x, 7, 2) << 6))
 117 #define RVC_SDSP_IMM(x)                 ((RV_X(x, 10, 3) << 3) | \
 118                                          (RV_X(x, 7, 3) << 6))
 119 #define RVC_RS1S(insn)                  (8 + RV_X(insn, SH_RD, 3))
 120 #define RVC_RS2S(insn)                  (8 + RV_X(insn, SH_RS2C, 3))
 121 #define RVC_RS2(insn)                   RV_X(insn, SH_RS2C, 5)
 122
 123 #define SHIFT_RIGHT(x, y)               \
 124         ((y) < 0 ? ((x) << -(y)) : ((x) >> (y)))
 125
 126 #define REG_MASK                        \
 127         ((1 << (5 + LOG_REGBYTES)) - (1 << LOG_REGBYTES))
 128
 129 #define REG_OFFSET(insn, pos)           \
 130         (SHIFT_RIGHT((insn), (pos) - LOG_REGBYTES) & REG_MASK)
 131
 132 #define REG_PTR(insn, pos, regs)        \
 133         (ulong *)((ulong)(regs) + REG_OFFSET(insn, pos))
 134
 135 #define GET_RM(insn)                    (((insn) >> 12) & 7)
 136
 137 #define GET_RS1(insn, regs)             (*REG_PTR(insn, SH_RS1, regs))
 138 #define GET_RS2(insn, regs)             (*REG_PTR(insn, SH_RS2, regs))
 139 #define GET_RS1S(insn, regs)            (*REG_PTR(RVC_RS1S(insn), 0, regs))
 140 #define GET_RS2S(insn, regs)            (*REG_PTR(RVC_RS2S(insn), 0, regs))
 141 #define GET_RS2C(insn, regs)            (*REG_PTR(insn, SH_RS2C, regs))
 142 #define GET_SP(regs)                    (*REG_PTR(2, 0, regs))
 143 #define SET_RD(insn, regs, val)         (*REG_PTR(insn, SH_RD, regs) = (val))
 144 #define IMM_I(insn)                     ((s32)(insn) >> 20)
 145 #define IMM_S(insn)                     (((s32)(insn) >> 25 << 5) | \
 146                                          (s32)(((insn) >> 7) & 0x1f))
 147 #define MASK_FUNCT3                     0x7000
 148
 149 #define GET_PRECISION(insn) (((insn) >> 25) & 3)
 150 #define GET_RM(insn) (((insn) >> 12) & 7)
 151 #define PRECISION_S 0
 152 #define PRECISION_D 1
 153
 154 #define DECLARE_UNPRIVILEGED_LOAD_FUNCTION(type, insn)                  \
 155 static inline type load_##type(const type *addr)                        \
 156 {                                                                       \
 157         type val;                                                       \
 158         asm (#insn " %0, %1"                                            \
 159         : "=&r" (val) : "m" (*addr));                                   \
 160         return val;                                                     \
 161 }
 162
 163 #define DECLARE_UNPRIVILEGED_STORE_FUNCTION(type, insn)                 \
 164 static inline void store_##type(type *addr, type val)                   \
 165 {                                                                       \
 166         asm volatile (#insn " %0, %1\n"                                 \
 167         : : "r" (val), "m" (*addr));                                    \
 168 }
 169
 170 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u8, lbu)
 171 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u16, lhu)
 172 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(s8, lb)
 173 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(s16, lh)
 174 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(s32, lw)
 175 DECLARE_UNPRIVILEGED_STORE_FUNCTION(u8, sb)
 176 DECLARE_UNPRIVILEGED_STORE_FUNCTION(u16, sh)
 177 DECLARE_UNPRIVILEGED_STORE_FUNCTION(u32, sw)
 178 #if defined(CONFIG_64BIT)
 179 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u32, lwu)
 180 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u64, ld)
 181 DECLARE_UNPRIVILEGED_STORE_FUNCTION(u64, sd)
 182 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(ulong, ld)
 183 #else
 184 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u32, lw)
 185 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(ulong, lw)
 186
 187 static inline u64 load_u64(const u64 *addr)
 188 {
 189         return load_u32((u32 *)addr)
 190                 + ((u64)load_u32((u32 *)addr + 1) << 32);
 191 }
 192
 193 static inline void store_u64(u64 *addr, u64 val)
 194 {
 195         store_u32((u32 *)addr, val);
 196         store_u32((u32 *)addr + 1, val >> 32);
 197 }
 198 #endif
 199
 200 static inline ulong get_insn(ulong mepc)
 201 {
 202         register ulong __mepc asm ("a2") = mepc;
 203         ulong val, rvc_mask = 3, tmp;
 204
 205         asm ("and %[tmp], %[addr], 2\n"
 206                 "bnez %[tmp], 1f\n"
 207 #if defined(CONFIG_64BIT)
 208                 __stringify(LWU) " %[insn], (%[addr])\n"
 209 #else
 210                 __stringify(LW) " %[insn], (%[addr])\n"
 211 #endif
 212                 "and %[tmp], %[insn], %[rvc_mask]\n"
 213                 "beq %[tmp], %[rvc_mask], 2f\n"
 214                 "sll %[insn], %[insn], %[xlen_minus_16]\n"
 215                 "srl %[insn], %[insn], %[xlen_minus_16]\n"
 216                 "j 2f\n"
 217                 "1:\n"
 218                 "lhu %[insn], (%[addr])\n"
 219                 "and %[tmp], %[insn], %[rvc_mask]\n"
 220                 "bne %[tmp], %[rvc_mask], 2f\n"
 221                 "lhu %[tmp], 2(%[addr])\n"
 222                 "sll %[tmp], %[tmp], 16\n"
 223                 "add %[insn], %[insn], %[tmp]\n"
 224                 "2:"
 225         : [insn] "=&r" (val), [tmp] "=&r" (tmp)
 226         : [addr] "r" (__mepc), [rvc_mask] "r" (rvc_mask),
 227           [xlen_minus_16] "i" (XLEN_MINUS_16));
 228
 229         return val;
 230 }
 231
 232 union reg_data {
 233         u8 data_bytes[8];
 234         ulong data_ulong;
 235         u64 data_u64;
 236 };
 237
 238 int handle_misaligned_load(struct pt_regs *regs)
 239 {
 240         union reg_data val;
 241         unsigned long epc = regs->epc;
 242         unsigned long insn = get_insn(epc);
 243         unsigned long addr = csr_read(mtval);
 244         int i, fp = 0, shift = 0, len = 0;
 245
 246         regs->epc = 0;
 247
 248         if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
 249                 len = 4;
 250                 shift = 8 * (sizeof(unsigned long) - len);
 251 #if defined(CONFIG_64BIT)
 252         } else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
 253                 len = 8;
 254                 shift = 8 * (sizeof(unsigned long) - len);
 255         } else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
 256                 len = 4;
 257 #endif
 258         } else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
 259                 fp = 1;
 260                 len = 8;
 261         } else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
 262                 fp = 1;
 263                 len = 4;
 264         } else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) {
 265                 len = 2;
 266                 shift = 8 * (sizeof(unsigned long) - len);
 267         } else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) {
 268                 len = 2;
 269 #if defined(CONFIG_64BIT)
 270         } else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
 271                 len = 8;
 272                 shift = 8 * (sizeof(unsigned long) - len);
 273                 insn = RVC_RS2S(insn) << SH_RD;
 274         } else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
 275                    ((insn >> SH_RD) & 0x1f)) {
 276                 len = 8;
 277                 shift = 8 * (sizeof(unsigned long) - len);
 278 #endif
 279         } else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) {
 280                 len = 4;
 281                 shift = 8 * (sizeof(unsigned long) - len);
 282                 insn = RVC_RS2S(insn) << SH_RD;
 283         } else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP &&
 284                    ((insn >> SH_RD) & 0x1f)) {
 285                 len = 4;
 286                 shift = 8 * (sizeof(unsigned long) - len);
 287         } else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) {
 288                 fp = 1;
 289                 len = 8;
 290                 insn = RVC_RS2S(insn) << SH_RD;
 291         } else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) {
 292                 fp = 1;
 293                 len = 8;
 294 #if defined(CONFIG_32BIT)
 295         } else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
 296                 fp = 1;
 297                 len = 4;
 298                 insn = RVC_RS2S(insn) << SH_RD;
 299         } else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
 300                 fp = 1;
 301                 len = 4;
 302 #endif
 303         } else {
 304                 regs->epc = epc;
 305                 return -1;
 306         }
 307
 308         val.data_u64 = 0;
 309         for (i = 0; i < len; i++)
 310                 val.data_bytes[i] = load_u8((void *)(addr + i));
 311
 312         if (fp)
 313                 return -1;
 314         SET_RD(insn, regs, val.data_ulong << shift >> shift);
 315
 316         regs->epc = epc + INSN_LEN(insn);
 317
 318         return 0;
 319 }
 320
 321 int handle_misaligned_store(struct pt_regs *regs)
 322 {
 323         union reg_data val;
 324         unsigned long epc = regs->epc;
 325         unsigned long insn = get_insn(epc);
 326         unsigned long addr = csr_read(mtval);
 327         int i, len = 0;
 328
 329         regs->epc = 0;
 330
 331         val.data_ulong = GET_RS2(insn, regs);
 332
 333         if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
 334                 len = 4;
 335 #if defined(CONFIG_64BIT)
 336         } else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
 337                 len = 8;
 338 #endif
 339         } else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) {
 340                 len = 2;
 341 #if defined(CONFIG_64BIT)
 342         } else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
 343                 len = 8;
 344                 val.data_ulong = GET_RS2S(insn, regs);
 345         } else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP) {
 346                 len = 8;
 347                 val.data_ulong = GET_RS2C(insn, regs);
 348 #endif
 349         } else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) {
 350                 len = 4;
 351                 val.data_ulong = GET_RS2S(insn, regs);
 352         } else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP) {
 353                 len = 4;
 354                 val.data_ulong = GET_RS2C(insn, regs);
 355         } else {
 356                 regs->epc = epc;
 357                 return -1;
 358         }
 359
 360         for (i = 0; i < len; i++)
 361                 store_u8((void *)(addr + i), val.data_bytes[i]);
 362
 363         regs->epc = epc + INSN_LEN(insn);
 364
 365         return 0;
 366 }