arch/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/perf_event.h>
  10 #include <linux/irq.h>
  11 #include <linux/stringify.h>
  12
  13 #include <asm/processor.h>
  14 #include <asm/ptrace.h>
  15 #include <asm/csr.h>
  16 #include <asm/entry-common.h>
  17 #include <asm/hwprobe.h>
  18 #include <asm/cpufeature.h>
  19
  20 #define INSN_MATCH_LB                   0x3
  21 #define INSN_MASK_LB                    0x707f
  22 #define INSN_MATCH_LH                   0x1003
  23 #define INSN_MASK_LH                    0x707f
  24 #define INSN_MATCH_LW                   0x2003
  25 #define INSN_MASK_LW                    0x707f
  26 #define INSN_MATCH_LD                   0x3003
  27 #define INSN_MASK_LD                    0x707f
  28 #define INSN_MATCH_LBU                  0x4003
  29 #define INSN_MASK_LBU                   0x707f
  30 #define INSN_MATCH_LHU                  0x5003
  31 #define INSN_MASK_LHU                   0x707f
  32 #define INSN_MATCH_LWU                  0x6003
  33 #define INSN_MASK_LWU                   0x707f
  34 #define INSN_MATCH_SB                   0x23
  35 #define INSN_MASK_SB                    0x707f
  36 #define INSN_MATCH_SH                   0x1023
  37 #define INSN_MASK_SH                    0x707f
  38 #define INSN_MATCH_SW                   0x2023
  39 #define INSN_MASK_SW                    0x707f
  40 #define INSN_MATCH_SD                   0x3023
  41 #define INSN_MASK_SD                    0x707f
  42
  43 #define INSN_MATCH_FLW                  0x2007
  44 #define INSN_MASK_FLW                   0x707f
  45 #define INSN_MATCH_FLD                  0x3007
  46 #define INSN_MASK_FLD                   0x707f
  47 #define INSN_MATCH_FLQ                  0x4007
  48 #define INSN_MASK_FLQ                   0x707f
  49 #define INSN_MATCH_FSW                  0x2027
  50 #define INSN_MASK_FSW                   0x707f
  51 #define INSN_MATCH_FSD                  0x3027
  52 #define INSN_MASK_FSD                   0x707f
  53 #define INSN_MATCH_FSQ                  0x4027
  54 #define INSN_MASK_FSQ                   0x707f
  55
  56 #define INSN_MATCH_C_LD                 0x6000
  57 #define INSN_MASK_C_LD                  0xe003
  58 #define INSN_MATCH_C_SD                 0xe000
  59 #define INSN_MASK_C_SD                  0xe003
  60 #define INSN_MATCH_C_LW                 0x4000
  61 #define INSN_MASK_C_LW                  0xe003
  62 #define INSN_MATCH_C_SW                 0xc000
  63 #define INSN_MASK_C_SW                  0xe003
  64 #define INSN_MATCH_C_LDSP               0x6002
  65 #define INSN_MASK_C_LDSP                0xe003
  66 #define INSN_MATCH_C_SDSP               0xe002
  67 #define INSN_MASK_C_SDSP                0xe003
  68 #define INSN_MATCH_C_LWSP               0x4002
  69 #define INSN_MASK_C_LWSP                0xe003
  70 #define INSN_MATCH_C_SWSP               0xc002
  71 #define INSN_MASK_C_SWSP                0xe003
  72
  73 #define INSN_MATCH_C_FLD                0x2000
  74 #define INSN_MASK_C_FLD                 0xe003
  75 #define INSN_MATCH_C_FLW                0x6000
  76 #define INSN_MASK_C_FLW                 0xe003
  77 #define INSN_MATCH_C_FSD                0xa000
  78 #define INSN_MASK_C_FSD                 0xe003
  79 #define INSN_MATCH_C_FSW                0xe000
  80 #define INSN_MASK_C_FSW                 0xe003
  81 #define INSN_MATCH_C_FLDSP              0x2002
  82 #define INSN_MASK_C_FLDSP               0xe003
  83 #define INSN_MATCH_C_FSDSP              0xa002
  84 #define INSN_MASK_C_FSDSP               0xe003
  85 #define INSN_MATCH_C_FLWSP              0x6002
  86 #define INSN_MASK_C_FLWSP               0xe003
  87 #define INSN_MATCH_C_FSWSP              0xe002
  88 #define INSN_MASK_C_FSWSP               0xe003
  89
  90 #define INSN_LEN(insn)                  ((((insn) & 0x3) < 0x3) ? 2 : 4)
  91
  92 #if defined(CONFIG_64BIT)
  93 #define LOG_REGBYTES                    3
  94 #define XLEN                            64
  95 #else
  96 #define LOG_REGBYTES                    2
  97 #define XLEN                            32
  98 #endif
  99 #define REGBYTES                        (1 << LOG_REGBYTES)
 100 #define XLEN_MINUS_16                   ((XLEN) - 16)
 101
 102 #define SH_RD                           7
 103 #define SH_RS1                          15
 104 #define SH_RS2                          20
 105 #define SH_RS2C                         2
 106
 107 #define RV_X(x, s, n)                   (((x) >> (s)) & ((1 << (n)) - 1))
 108 #define RVC_LW_IMM(x)                   ((RV_X(x, 6, 1) << 2) | \
 109                                          (RV_X(x, 10, 3) << 3) | \
 110                                          (RV_X(x, 5, 1) << 6))
 111 #define RVC_LD_IMM(x)                   ((RV_X(x, 10, 3) << 3) | \
 112                                          (RV_X(x, 5, 2) << 6))
 113 #define RVC_LWSP_IMM(x)                 ((RV_X(x, 4, 3) << 2) | \
 114                                          (RV_X(x, 12, 1) << 5) | \
 115                                          (RV_X(x, 2, 2) << 6))
 116 #define RVC_LDSP_IMM(x)                 ((RV_X(x, 5, 2) << 3) | \
 117                                          (RV_X(x, 12, 1) << 5) | \
 118                                          (RV_X(x, 2, 3) << 6))
 119 #define RVC_SWSP_IMM(x)                 ((RV_X(x, 9, 4) << 2) | \
 120                                          (RV_X(x, 7, 2) << 6))
 121 #define RVC_SDSP_IMM(x)                 ((RV_X(x, 10, 3) << 3) | \
 122                                          (RV_X(x, 7, 3) << 6))
 123 #define RVC_RS1S(insn)                  (8 + RV_X(insn, SH_RD, 3))
 124 #define RVC_RS2S(insn)                  (8 + RV_X(insn, SH_RS2C, 3))
 125 #define RVC_RS2(insn)                   RV_X(insn, SH_RS2C, 5)
 126
 127 #define SHIFT_RIGHT(x, y)               \
 128         ((y) < 0 ? ((x) << -(y)) : ((x) >> (y)))
 129
 130 #define REG_MASK                        \
 131         ((1 << (5 + LOG_REGBYTES)) - (1 << LOG_REGBYTES))
 132
 133 #define REG_OFFSET(insn, pos)           \
 134         (SHIFT_RIGHT((insn), (pos) - LOG_REGBYTES) & REG_MASK)
 135
 136 #define REG_PTR(insn, pos, regs)        \
 137         (ulong *)((ulong)(regs) + REG_OFFSET(insn, pos))
 138
 139 #define GET_RM(insn)                    (((insn) >> 12) & 7)
 140
 141 #define GET_RS1(insn, regs)             (*REG_PTR(insn, SH_RS1, regs))
 142 #define GET_RS2(insn, regs)             (*REG_PTR(insn, SH_RS2, regs))
 143 #define GET_RS1S(insn, regs)            (*REG_PTR(RVC_RS1S(insn), 0, regs))
 144 #define GET_RS2S(insn, regs)            (*REG_PTR(RVC_RS2S(insn), 0, regs))
 145 #define GET_RS2C(insn, regs)            (*REG_PTR(insn, SH_RS2C, regs))
 146 #define GET_SP(regs)                    (*REG_PTR(2, 0, regs))
 147 #define SET_RD(insn, regs, val)         (*REG_PTR(insn, SH_RD, regs) = (val))
 148 #define IMM_I(insn)                     ((s32)(insn) >> 20)
 149 #define IMM_S(insn)                     (((s32)(insn) >> 25 << 5) | \
 150                                          (s32)(((insn) >> 7) & 0x1f))
 151 #define MASK_FUNCT3                     0x7000
 152
 153 #define GET_PRECISION(insn) (((insn) >> 25) & 3)
 154 #define GET_RM(insn) (((insn) >> 12) & 7)
 155 #define PRECISION_S 0
 156 #define PRECISION_D 1
 157
 158 #ifdef CONFIG_FPU
 159
 160 #define FP_GET_RD(insn)         (insn >> 7 & 0x1F)
 161
 162 extern void put_f32_reg(unsigned long fp_reg, unsigned long value);
 163
 164 static int set_f32_rd(unsigned long insn, struct pt_regs *regs,
 165                       unsigned long val)
 166 {
 167         unsigned long fp_reg = FP_GET_RD(insn);
 168
 169         put_f32_reg(fp_reg, val);
 170         regs->status |= SR_FS_DIRTY;
 171
 172         return 0;
 173 }
 174
 175 extern void put_f64_reg(unsigned long fp_reg, unsigned long value);
 176
 177 static int set_f64_rd(unsigned long insn, struct pt_regs *regs, u64 val)
 178 {
 179         unsigned long fp_reg = FP_GET_RD(insn);
 180         unsigned long value;
 181
 182 #if __riscv_xlen == 32
 183         value = (unsigned long) &val;
 184 #else
 185         value = val;
 186 #endif
 187         put_f64_reg(fp_reg, value);
 188         regs->status |= SR_FS_DIRTY;
 189
 190         return 0;
 191 }
 192
 193 #if __riscv_xlen == 32
 194 extern void get_f64_reg(unsigned long fp_reg, u64 *value);
 195
 196 static u64 get_f64_rs(unsigned long insn, u8 fp_reg_offset,
 197                       struct pt_regs *regs)
 198 {
 199         unsigned long fp_reg = (insn >> fp_reg_offset) & 0x1F;
 200         u64 val;
 201
 202         get_f64_reg(fp_reg, &val);
 203         regs->status |= SR_FS_DIRTY;
 204
 205         return val;
 206 }
 207 #else
 208
 209 extern unsigned long get_f64_reg(unsigned long fp_reg);
 210
 211 static unsigned long get_f64_rs(unsigned long insn, u8 fp_reg_offset,
 212                                 struct pt_regs *regs)
 213 {
 214         unsigned long fp_reg = (insn >> fp_reg_offset) & 0x1F;
 215         unsigned long val;
 216
 217         val = get_f64_reg(fp_reg);
 218         regs->status |= SR_FS_DIRTY;
 219
 220         return val;
 221 }
 222
 223 #endif
 224
 225 extern unsigned long get_f32_reg(unsigned long fp_reg);
 226
 227 static unsigned long get_f32_rs(unsigned long insn, u8 fp_reg_offset,
 228                                 struct pt_regs *regs)
 229 {
 230         unsigned long fp_reg = (insn >> fp_reg_offset) & 0x1F;
 231         unsigned long val;
 232
 233         val = get_f32_reg(fp_reg);
 234         regs->status |= SR_FS_DIRTY;
 235
 236         return val;
 237 }
 238
 239 #else /* CONFIG_FPU */
 240 static void set_f32_rd(unsigned long insn, struct pt_regs *regs,
 241                        unsigned long val) {}
 242
 243 static void set_f64_rd(unsigned long insn, struct pt_regs *regs, u64 val) {}
 244
 245 static unsigned long get_f64_rs(unsigned long insn, u8 fp_reg_offset,
 246                                 struct pt_regs *regs)
 247 {
 248         return 0;
 249 }
 250
 251 static unsigned long get_f32_rs(unsigned long insn, u8 fp_reg_offset,
 252                                 struct pt_regs *regs)
 253 {
 254         return 0;
 255 }
 256
 257 #endif
 258
 259 #define GET_F64_RS2(insn, regs) (get_f64_rs(insn, 20, regs))
 260 #define GET_F64_RS2C(insn, regs) (get_f64_rs(insn, 2, regs))
 261 #define GET_F64_RS2S(insn, regs) (get_f64_rs(RVC_RS2S(insn), 0, regs))
 262
 263 #define GET_F32_RS2(insn, regs) (get_f32_rs(insn, 20, regs))
 264 #define GET_F32_RS2C(insn, regs) (get_f32_rs(insn, 2, regs))
 265 #define GET_F32_RS2S(insn, regs) (get_f32_rs(RVC_RS2S(insn), 0, regs))
 266
 267 #ifdef CONFIG_RISCV_M_MODE
 268 static inline int load_u8(struct pt_regs *regs, const u8 *addr, u8 *r_val)
 269 {
 270         u8 val;
 271
 272         asm volatile("lbu %0, %1" : "=&r" (val) : "m" (*addr));
 273         *r_val = val;
 274
 275         return 0;
 276 }
 277
 278 static inline int store_u8(struct pt_regs *regs, u8 *addr, u8 val)
 279 {
 280         asm volatile ("sb %0, %1\n" : : "r" (val), "m" (*addr));
 281
 282         return 0;
 283 }
 284
 285 static inline int get_insn(struct pt_regs *regs, ulong mepc, ulong *r_insn)
 286 {
 287         register ulong __mepc asm ("a2") = mepc;
 288         ulong val, rvc_mask = 3, tmp;
 289
 290         asm ("and %[tmp], %[addr], 2\n"
 291                 "bnez %[tmp], 1f\n"
 292 #if defined(CONFIG_64BIT)
 293                 __stringify(LWU) " %[insn], (%[addr])\n"
 294 #else
 295                 __stringify(LW) " %[insn], (%[addr])\n"
 296 #endif
 297                 "and %[tmp], %[insn], %[rvc_mask]\n"
 298                 "beq %[tmp], %[rvc_mask], 2f\n"
 299                 "sll %[insn], %[insn], %[xlen_minus_16]\n"
 300                 "srl %[insn], %[insn], %[xlen_minus_16]\n"
 301                 "j 2f\n"
 302                 "1:\n"
 303                 "lhu %[insn], (%[addr])\n"
 304                 "and %[tmp], %[insn], %[rvc_mask]\n"
 305                 "bne %[tmp], %[rvc_mask], 2f\n"
 306                 "lhu %[tmp], 2(%[addr])\n"
 307                 "sll %[tmp], %[tmp], 16\n"
 308                 "add %[insn], %[insn], %[tmp]\n"
 309                 "2:"
 310         : [insn] "=&r" (val), [tmp] "=&r" (tmp)
 311         : [addr] "r" (__mepc), [rvc_mask] "r" (rvc_mask),
 312           [xlen_minus_16] "i" (XLEN_MINUS_16));
 313
 314         *r_insn = val;
 315
 316         return 0;
 317 }
 318 #else
 319 static inline int load_u8(struct pt_regs *regs, const u8 *addr, u8 *r_val)
 320 {
 321         if (user_mode(regs)) {
 322                 return __get_user(*r_val, addr);
 323         } else {
 324                 *r_val = *addr;
 325                 return 0;
 326         }
 327 }
 328
 329 static inline int store_u8(struct pt_regs *regs, u8 *addr, u8 val)
 330 {
 331         if (user_mode(regs)) {
 332                 return __put_user(val, addr);
 333         } else {
 334                 *addr = val;
 335                 return 0;
 336         }
 337 }
 338
 339 #define __read_insn(regs, insn, insn_addr)              \
 340 ({                                                      \
 341         int __ret;                                      \
 342                                                         \
 343         if (user_mode(regs)) {                          \
 344                 __ret = __get_user(insn, insn_addr);    \
 345         } else {                                        \
 346                 insn = *insn_addr;                      \
 347                 __ret = 0;                              \
 348         }                                               \
 349                                                         \
 350         __ret;                                          \
 351 })
 352
 353 static inline int get_insn(struct pt_regs *regs, ulong epc, ulong *r_insn)
 354 {
 355         ulong insn = 0;
 356
 357         if (epc & 0x2) {
 358                 ulong tmp = 0;
 359                 u16 __user *insn_addr = (u16 __user *)epc;
 360
 361                 if (__read_insn(regs, insn, insn_addr))
 362                         return -EFAULT;
 363                 /* __get_user() uses regular "lw" which sign extend the loaded
 364                  * value make sure to clear higher order bits in case we "or" it
 365                  * below with the upper 16 bits half.
 366                  */
 367                 insn &= GENMASK(15, 0);
 368                 if ((insn & __INSN_LENGTH_MASK) != __INSN_LENGTH_32) {
 369                         *r_insn = insn;
 370                         return 0;
 371                 }
 372                 insn_addr++;
 373                 if (__read_insn(regs, tmp, insn_addr))
 374                         return -EFAULT;
 375                 *r_insn = (tmp << 16) | insn;
 376
 377                 return 0;
 378         } else {
 379                 u32 __user *insn_addr = (u32 __user *)epc;
 380
 381                 if (__read_insn(regs, insn, insn_addr))
 382                         return -EFAULT;
 383                 if ((insn & __INSN_LENGTH_MASK) == __INSN_LENGTH_32) {
 384                         *r_insn = insn;
 385                         return 0;
 386                 }
 387                 insn &= GENMASK(15, 0);
 388                 *r_insn = insn;
 389
 390                 return 0;
 391         }
 392 }
 393 #endif
 394
 395 union reg_data {
 396         u8 data_bytes[8];
 397         ulong data_ulong;
 398         u64 data_u64;
 399 };
 400
 401 static bool unaligned_ctl __read_mostly;
 402
 403 /* sysctl hooks */
 404 int unaligned_enabled __read_mostly = 1;        /* Enabled by default */
 405
 406 int handle_misaligned_load(struct pt_regs *regs)
 407 {
 408         union reg_data val;
 409         unsigned long epc = regs->epc;
 410         unsigned long insn;
 411         unsigned long addr = regs->badaddr;
 412         int i, fp = 0, shift = 0, len = 0;
 413
 414         perf_sw_event(PERF_COUNT_SW_ALIGNMENT_FAULTS, 1, regs, addr);
 415
 416         *this_cpu_ptr(&misaligned_access_speed) = RISCV_HWPROBE_MISALIGNED_EMULATED;
 417
 418         if (!unaligned_enabled)
 419                 return -1;
 420
 421         if (user_mode(regs) && (current->thread.align_ctl & PR_UNALIGN_SIGBUS))
 422                 return -1;
 423
 424         if (get_insn(regs, epc, &insn))
 425                 return -1;
 426
 427         regs->epc = 0;
 428
 429         if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
 430                 len = 4;
 431                 shift = 8 * (sizeof(unsigned long) - len);
 432 #if defined(CONFIG_64BIT)
 433         } else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
 434                 len = 8;
 435                 shift = 8 * (sizeof(unsigned long) - len);
 436         } else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
 437                 len = 4;
 438 #endif
 439         } else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
 440                 fp = 1;
 441                 len = 8;
 442         } else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
 443                 fp = 1;
 444                 len = 4;
 445         } else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) {
 446                 len = 2;
 447                 shift = 8 * (sizeof(unsigned long) - len);
 448         } else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) {
 449                 len = 2;
 450 #if defined(CONFIG_64BIT)
 451         } else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
 452                 len = 8;
 453                 shift = 8 * (sizeof(unsigned long) - len);
 454                 insn = RVC_RS2S(insn) << SH_RD;
 455         } else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
 456                    ((insn >> SH_RD) & 0x1f)) {
 457                 len = 8;
 458                 shift = 8 * (sizeof(unsigned long) - len);
 459 #endif
 460         } else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) {
 461                 len = 4;
 462                 shift = 8 * (sizeof(unsigned long) - len);
 463                 insn = RVC_RS2S(insn) << SH_RD;
 464         } else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP &&
 465                    ((insn >> SH_RD) & 0x1f)) {
 466                 len = 4;
 467                 shift = 8 * (sizeof(unsigned long) - len);
 468         } else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) {
 469                 fp = 1;
 470                 len = 8;
 471                 insn = RVC_RS2S(insn) << SH_RD;
 472         } else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) {
 473                 fp = 1;
 474                 len = 8;
 475 #if defined(CONFIG_32BIT)
 476         } else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
 477                 fp = 1;
 478                 len = 4;
 479                 insn = RVC_RS2S(insn) << SH_RD;
 480         } else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
 481                 fp = 1;
 482                 len = 4;
 483 #endif
 484         } else {
 485                 regs->epc = epc;
 486                 return -1;
 487         }
 488
 489         if (!IS_ENABLED(CONFIG_FPU) && fp)
 490                 return -EOPNOTSUPP;
 491
 492         val.data_u64 = 0;
 493         for (i = 0; i < len; i++) {
 494                 if (load_u8(regs, (void *)(addr + i), &val.data_bytes[i]))
 495                         return -1;
 496         }
 497
 498         if (!fp)
 499                 SET_RD(insn, regs, val.data_ulong << shift >> shift);
 500         else if (len == 8)
 501                 set_f64_rd(insn, regs, val.data_u64);
 502         else
 503                 set_f32_rd(insn, regs, val.data_ulong);
 504
 505         regs->epc = epc + INSN_LEN(insn);
 506
 507         return 0;
 508 }
 509
 510 int handle_misaligned_store(struct pt_regs *regs)
 511 {
 512         union reg_data val;
 513         unsigned long epc = regs->epc;
 514         unsigned long insn;
 515         unsigned long addr = regs->badaddr;
 516         int i, len = 0, fp = 0;
 517
 518         perf_sw_event(PERF_COUNT_SW_ALIGNMENT_FAULTS, 1, regs, addr);
 519
 520         if (!unaligned_enabled)
 521                 return -1;
 522
 523         if (user_mode(regs) && (current->thread.align_ctl & PR_UNALIGN_SIGBUS))
 524                 return -1;
 525
 526         if (get_insn(regs, epc, &insn))
 527                 return -1;
 528
 529         regs->epc = 0;
 530
 531         val.data_ulong = GET_RS2(insn, regs);
 532
 533         if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
 534                 len = 4;
 535 #if defined(CONFIG_64BIT)
 536         } else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
 537                 len = 8;
 538 #endif
 539         } else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) {
 540                 fp = 1;
 541                 len = 8;
 542                 val.data_u64 = GET_F64_RS2(insn, regs);
 543         } else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) {
 544                 fp = 1;
 545                 len = 4;
 546                 val.data_ulong = GET_F32_RS2(insn, regs);
 547         } else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) {
 548                 len = 2;
 549 #if defined(CONFIG_64BIT)
 550         } else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
 551                 len = 8;
 552                 val.data_ulong = GET_RS2S(insn, regs);
 553         } else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP) {
 554                 len = 8;
 555                 val.data_ulong = GET_RS2C(insn, regs);
 556 #endif
 557         } else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) {
 558                 len = 4;
 559                 val.data_ulong = GET_RS2S(insn, regs);
 560         } else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP) {
 561                 len = 4;
 562                 val.data_ulong = GET_RS2C(insn, regs);
 563         } else if ((insn & INSN_MASK_C_FSD) == INSN_MATCH_C_FSD) {
 564                 fp = 1;
 565                 len = 8;
 566                 val.data_u64 = GET_F64_RS2S(insn, regs);
 567         } else if ((insn & INSN_MASK_C_FSDSP) == INSN_MATCH_C_FSDSP) {
 568                 fp = 1;
 569                 len = 8;
 570                 val.data_u64 = GET_F64_RS2C(insn, regs);
 571 #if !defined(CONFIG_64BIT)
 572         } else if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) {
 573                 fp = 1;
 574                 len = 4;
 575                 val.data_ulong = GET_F32_RS2S(insn, regs);
 576         } else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) {
 577                 fp = 1;
 578                 len = 4;
 579                 val.data_ulong = GET_F32_RS2C(insn, regs);
 580 #endif
 581         } else {
 582                 regs->epc = epc;
 583                 return -1;
 584         }
 585
 586         if (!IS_ENABLED(CONFIG_FPU) && fp)
 587                 return -EOPNOTSUPP;
 588
 589         for (i = 0; i < len; i++) {
 590                 if (store_u8(regs, (void *)(addr + i), val.data_bytes[i]))
 591                         return -1;
 592         }
 593
 594         regs->epc = epc + INSN_LEN(insn);
 595
 596         return 0;
 597 }
 598
 599 bool check_unaligned_access_emulated(int cpu)
 600 {
 601         long *mas_ptr = per_cpu_ptr(&misaligned_access_speed, cpu);
 602         unsigned long tmp_var, tmp_val;
 603         bool misaligned_emu_detected;
 604
 605         *mas_ptr = RISCV_HWPROBE_MISALIGNED_UNKNOWN;
 606
 607         __asm__ __volatile__ (
 608                 "       "REG_L" %[tmp], 1(%[ptr])\n"
 609                 : [tmp] "=r" (tmp_val) : [ptr] "r" (&tmp_var) : "memory");
 610
 611         misaligned_emu_detected = (*mas_ptr == RISCV_HWPROBE_MISALIGNED_EMULATED);
 612         /*
 613          * If unaligned_ctl is already set, this means that we detected that all
 614          * CPUS uses emulated misaligned access at boot time. If that changed
 615          * when hotplugging the new cpu, this is something we don't handle.
 616          */
 617         if (unlikely(unaligned_ctl && !misaligned_emu_detected)) {
 618                 pr_crit("CPU misaligned accesses non homogeneous (expected all emulated)\n");
 619                 while (true)
 620                         cpu_relax();
 621         }
 622
 623         return misaligned_emu_detected;
 624 }
 625
 626 void unaligned_emulation_finish(void)
 627 {
 628         int cpu;
 629
 630         /*
 631          * We can only support PR_UNALIGN controls if all CPUs have misaligned
 632          * accesses emulated since tasks requesting such control can run on any
 633          * CPU.
 634          */
 635         for_each_present_cpu(cpu) {
 636                 if (per_cpu(misaligned_access_speed, cpu) !=
 637                                         RISCV_HWPROBE_MISALIGNED_EMULATED) {
 638                         return;
 639                 }
 640         }
 641         unaligned_ctl = true;
 642 }
 643
 644 bool unaligned_ctl_available(void)
 645 {
 646         return unaligned_ctl;
 647 }