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, (u8 __user *)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, (u8 __user *)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 = *(__force u16 *)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(event_id: PERF_COUNT_SW_ALIGNMENT_FAULTS, nr: 1, regs, addr); |
415 | |
416 | #ifdef CONFIG_RISCV_PROBE_UNALIGNED_ACCESS |
417 | *this_cpu_ptr(&misaligned_access_speed) = RISCV_HWPROBE_MISALIGNED_EMULATED; |
418 | #endif |
419 | |
420 | if (!unaligned_enabled) |
421 | return -1; |
422 | |
423 | if (user_mode(regs) && (current->thread.align_ctl & PR_UNALIGN_SIGBUS)) |
424 | return -1; |
425 | |
426 | if (get_insn(regs, epc, r_insn: &insn)) |
427 | return -1; |
428 | |
429 | regs->epc = 0; |
430 | |
431 | if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) { |
432 | len = 4; |
433 | shift = 8 * (sizeof(unsigned long) - len); |
434 | #if defined(CONFIG_64BIT) |
435 | } else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) { |
436 | len = 8; |
437 | shift = 8 * (sizeof(unsigned long) - len); |
438 | } else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) { |
439 | len = 4; |
440 | #endif |
441 | } else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) { |
442 | fp = 1; |
443 | len = 8; |
444 | } else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) { |
445 | fp = 1; |
446 | len = 4; |
447 | } else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) { |
448 | len = 2; |
449 | shift = 8 * (sizeof(unsigned long) - len); |
450 | } else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) { |
451 | len = 2; |
452 | #if defined(CONFIG_64BIT) |
453 | } else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) { |
454 | len = 8; |
455 | shift = 8 * (sizeof(unsigned long) - len); |
456 | insn = RVC_RS2S(insn) << SH_RD; |
457 | } else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP && |
458 | ((insn >> SH_RD) & 0x1f)) { |
459 | len = 8; |
460 | shift = 8 * (sizeof(unsigned long) - len); |
461 | #endif |
462 | } else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) { |
463 | len = 4; |
464 | shift = 8 * (sizeof(unsigned long) - len); |
465 | insn = RVC_RS2S(insn) << SH_RD; |
466 | } else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP && |
467 | ((insn >> SH_RD) & 0x1f)) { |
468 | len = 4; |
469 | shift = 8 * (sizeof(unsigned long) - len); |
470 | } else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) { |
471 | fp = 1; |
472 | len = 8; |
473 | insn = RVC_RS2S(insn) << SH_RD; |
474 | } else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) { |
475 | fp = 1; |
476 | len = 8; |
477 | #if defined(CONFIG_32BIT) |
478 | } else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) { |
479 | fp = 1; |
480 | len = 4; |
481 | insn = RVC_RS2S(insn) << SH_RD; |
482 | } else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) { |
483 | fp = 1; |
484 | len = 4; |
485 | #endif |
486 | } else { |
487 | regs->epc = epc; |
488 | return -1; |
489 | } |
490 | |
491 | if (!IS_ENABLED(CONFIG_FPU) && fp) |
492 | return -EOPNOTSUPP; |
493 | |
494 | val.data_u64 = 0; |
495 | for (i = 0; i < len; i++) { |
496 | if (load_u8(regs, addr: (void *)(addr + i), r_val: &val.data_bytes[i])) |
497 | return -1; |
498 | } |
499 | |
500 | if (!fp) |
501 | SET_RD(insn, regs, val.data_ulong << shift >> shift); |
502 | else if (len == 8) |
503 | set_f64_rd(insn, regs, val: val.data_u64); |
504 | else |
505 | set_f32_rd(insn, regs, val: val.data_ulong); |
506 | |
507 | regs->epc = epc + INSN_LEN(insn); |
508 | |
509 | return 0; |
510 | } |
511 | |
512 | int handle_misaligned_store(struct pt_regs *regs) |
513 | { |
514 | union reg_data val; |
515 | unsigned long epc = regs->epc; |
516 | unsigned long insn; |
517 | unsigned long addr = regs->badaddr; |
518 | int i, len = 0, fp = 0; |
519 | |
520 | perf_sw_event(event_id: PERF_COUNT_SW_ALIGNMENT_FAULTS, nr: 1, regs, addr); |
521 | |
522 | if (!unaligned_enabled) |
523 | return -1; |
524 | |
525 | if (user_mode(regs) && (current->thread.align_ctl & PR_UNALIGN_SIGBUS)) |
526 | return -1; |
527 | |
528 | if (get_insn(regs, epc, r_insn: &insn)) |
529 | return -1; |
530 | |
531 | regs->epc = 0; |
532 | |
533 | val.data_ulong = GET_RS2(insn, regs); |
534 | |
535 | if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) { |
536 | len = 4; |
537 | #if defined(CONFIG_64BIT) |
538 | } else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) { |
539 | len = 8; |
540 | #endif |
541 | } else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) { |
542 | fp = 1; |
543 | len = 8; |
544 | val.data_u64 = GET_F64_RS2(insn, regs); |
545 | } else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) { |
546 | fp = 1; |
547 | len = 4; |
548 | val.data_ulong = GET_F32_RS2(insn, regs); |
549 | } else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) { |
550 | len = 2; |
551 | #if defined(CONFIG_64BIT) |
552 | } else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) { |
553 | len = 8; |
554 | val.data_ulong = GET_RS2S(insn, regs); |
555 | } else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP) { |
556 | len = 8; |
557 | val.data_ulong = GET_RS2C(insn, regs); |
558 | #endif |
559 | } else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) { |
560 | len = 4; |
561 | val.data_ulong = GET_RS2S(insn, regs); |
562 | } else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP) { |
563 | len = 4; |
564 | val.data_ulong = GET_RS2C(insn, regs); |
565 | } else if ((insn & INSN_MASK_C_FSD) == INSN_MATCH_C_FSD) { |
566 | fp = 1; |
567 | len = 8; |
568 | val.data_u64 = GET_F64_RS2S(insn, regs); |
569 | } else if ((insn & INSN_MASK_C_FSDSP) == INSN_MATCH_C_FSDSP) { |
570 | fp = 1; |
571 | len = 8; |
572 | val.data_u64 = GET_F64_RS2C(insn, regs); |
573 | #if !defined(CONFIG_64BIT) |
574 | } else if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) { |
575 | fp = 1; |
576 | len = 4; |
577 | val.data_ulong = GET_F32_RS2S(insn, regs); |
578 | } else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) { |
579 | fp = 1; |
580 | len = 4; |
581 | val.data_ulong = GET_F32_RS2C(insn, regs); |
582 | #endif |
583 | } else { |
584 | regs->epc = epc; |
585 | return -1; |
586 | } |
587 | |
588 | if (!IS_ENABLED(CONFIG_FPU) && fp) |
589 | return -EOPNOTSUPP; |
590 | |
591 | for (i = 0; i < len; i++) { |
592 | if (store_u8(regs, addr: (void *)(addr + i), val: val.data_bytes[i])) |
593 | return -1; |
594 | } |
595 | |
596 | regs->epc = epc + INSN_LEN(insn); |
597 | |
598 | return 0; |
599 | } |
600 | |
601 | static bool check_unaligned_access_emulated(int cpu) |
602 | { |
603 | long *mas_ptr = per_cpu_ptr(&misaligned_access_speed, cpu); |
604 | unsigned long tmp_var, tmp_val; |
605 | bool misaligned_emu_detected; |
606 | |
607 | *mas_ptr = RISCV_HWPROBE_MISALIGNED_UNKNOWN; |
608 | |
609 | __asm__ __volatile__ ( |
610 | " " REG_L" %[tmp], 1(%[ptr])\n" |
611 | : [tmp] "=r" (tmp_val) : [ptr] "r" (&tmp_var) : "memory" ); |
612 | |
613 | misaligned_emu_detected = (*mas_ptr == RISCV_HWPROBE_MISALIGNED_EMULATED); |
614 | /* |
615 | * If unaligned_ctl is already set, this means that we detected that all |
616 | * CPUS uses emulated misaligned access at boot time. If that changed |
617 | * when hotplugging the new cpu, this is something we don't handle. |
618 | */ |
619 | if (unlikely(unaligned_ctl && !misaligned_emu_detected)) { |
620 | pr_crit("CPU misaligned accesses non homogeneous (expected all emulated)\n" ); |
621 | while (true) |
622 | cpu_relax(); |
623 | } |
624 | |
625 | return misaligned_emu_detected; |
626 | } |
627 | |
628 | bool check_unaligned_access_emulated_all_cpus(void) |
629 | { |
630 | int cpu; |
631 | |
632 | /* |
633 | * We can only support PR_UNALIGN controls if all CPUs have misaligned |
634 | * accesses emulated since tasks requesting such control can run on any |
635 | * CPU. |
636 | */ |
637 | for_each_online_cpu(cpu) |
638 | if (!check_unaligned_access_emulated(cpu)) |
639 | return false; |
640 | |
641 | unaligned_ctl = true; |
642 | return true; |
643 | } |
644 | |
645 | bool unaligned_ctl_available(void) |
646 | { |
647 | return unaligned_ctl; |
648 | } |
649 | |