xref: /linux/arch/riscv/kernel/traps_misaligned.c (revision ef9226cd56b718c79184a3466d32984a51cb449c)
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(PERF_COUNT_SW_ALIGNMENT_FAULTS, 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, &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, (void *)(addr + i), &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.data_u64);
504 	else
505 		set_f32_rd(insn, regs, 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(PERF_COUNT_SW_ALIGNMENT_FAULTS, 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, &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, (void *)(addr + i), 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