1 // SPDX-License-Identifier: GPL-2.0
2 // Copyright (C) 2005-2017 Andes Technology Corporation
4 #include <linux/proc_fs.h>
5 #include <linux/uaccess.h>
6 #include <linux/sysctl.h>
7 #include <asm/unaligned.h>
9 #define DEBUG(enable, tagged, ...) \
13 pr_warn("[ %30s() ] ", __func__); \
14 pr_warn(__VA_ARGS__); \
18 #define RT(inst) (((inst) >> 20) & 0x1FUL)
19 #define RA(inst) (((inst) >> 15) & 0x1FUL)
20 #define RB(inst) (((inst) >> 10) & 0x1FUL)
21 #define SV(inst) (((inst) >> 8) & 0x3UL)
22 #define IMM(inst) (((inst) >> 0) & 0x7FFFUL)
24 #define RA3(inst) (((inst) >> 3) & 0x7UL)
25 #define RT3(inst) (((inst) >> 6) & 0x7UL)
26 #define IMM3U(inst) (((inst) >> 0) & 0x7UL)
28 #define RA5(inst) (((inst) >> 0) & 0x1FUL)
29 #define RT4(inst) (((inst) >> 5) & 0xFUL)
31 #define GET_IMMSVAL(imm_value) \
32 (((imm_value >> 14) & 0x1) ? (imm_value - 0x8000) : imm_value)
34 #define __get8_data(val,addr,err) \
36 "1: lbi.bi %1, [%2], #1\n" \
38 " .pushsection .text.fixup,\"ax\"\n" \
43 " .pushsection __ex_table,\"a\"\n" \
47 : "=r" (err), "=&r" (val), "=r" (addr) \
48 : "0" (err), "2" (addr))
50 #define get16_data(addr, val_ptr) \
52 unsigned int err = 0, v, a = addr; \
53 __get8_data(v,a,err); \
55 __get8_data(v,a,err); \
59 *val_ptr = le16_to_cpu(*val_ptr); \
62 #define get32_data(addr, val_ptr) \
64 unsigned int err = 0, v, a = addr; \
65 __get8_data(v,a,err); \
67 __get8_data(v,a,err); \
69 __get8_data(v,a,err); \
70 *val_ptr |= v << 16; \
71 __get8_data(v,a,err); \
72 *val_ptr |= v << 24; \
75 *val_ptr = le32_to_cpu(*val_ptr); \
78 #define get_data(addr, val_ptr, len) \
80 get16_data(addr, val_ptr); \
82 get32_data(addr, val_ptr);
84 #define set16_data(addr, val) \
86 unsigned int err = 0, *ptr = addr ; \
87 val = le32_to_cpu(val); \
89 "1: sbi.bi %2, [%1], #1\n" \
90 " srli %2, %2, #8\n" \
93 " .pushsection .text.fixup,\"ax\"\n" \
98 " .pushsection __ex_table,\"a\"\n" \
103 : "=r" (err), "+r" (ptr), "+r" (val) \
110 #define set32_data(addr, val) \
112 unsigned int err = 0, *ptr = addr ; \
113 val = le32_to_cpu(val); \
115 "1: sbi.bi %2, [%1], #1\n" \
116 " srli %2, %2, #8\n" \
117 "2: sbi.bi %2, [%1], #1\n" \
118 " srli %2, %2, #8\n" \
119 "3: sbi.bi %2, [%1], #1\n" \
120 " srli %2, %2, #8\n" \
121 "4: sbi %2, [%1]\n" \
123 " .pushsection .text.fixup,\"ax\"\n" \
128 " .pushsection __ex_table,\"a\"\n" \
135 : "=r" (err), "+r" (ptr), "+r" (val) \
141 #define set_data(addr, val, len) \
143 set16_data(addr, val); \
145 set32_data(addr, val);
146 #define NDS32_16BIT_INSTRUCTION 0x80000000
148 extern pte_t va_present(struct mm_struct *mm, unsigned long addr);
149 extern pte_t va_kernel_present(unsigned long addr);
150 extern int va_readable(struct pt_regs *regs, unsigned long addr);
151 extern int va_writable(struct pt_regs *regs, unsigned long addr);
153 int unalign_access_mode = 0, unalign_access_debug = 0;
155 static inline unsigned long *idx_to_addr(struct pt_regs *regs, int idx)
157 /* this should be consistent with ptrace.h */
158 if (idx >= 0 && idx <= 25) /* R0-R25 */
159 return ®s->uregs[0] + idx;
160 else if (idx >= 28 && idx <= 30) /* FP, GP, LP */
161 return ®s->fp + (idx - 28);
162 else if (idx == 31) /* SP */
165 return NULL; /* cause a segfault */
168 static inline unsigned long get_inst(unsigned long addr)
170 return be32_to_cpu(get_unaligned((u32 *) addr));
173 static inline unsigned long sign_extend(unsigned long val, int len)
175 unsigned long ret = 0;
176 unsigned char *s, *t;
179 val = cpu_to_le32(val);
187 if (((*(t - 1)) & 0x80) && (i < 4)) {
193 return le32_to_cpu(ret);
196 static inline int do_16(unsigned long inst, struct pt_regs *regs)
198 int imm, regular, load, len, addr_mode, idx_mode;
199 unsigned long unaligned_addr, target_val, source_idx, target_idx,
201 switch ((inst >> 9) & 0x3F) {
203 case 0x12: /* LHI333 */
211 case 0x10: /* LWI333 */
219 case 0x11: /* LWI333.bi */
227 case 0x1A: /* LWI450 */
235 case 0x16: /* SHI333 */
243 case 0x14: /* SWI333 */
251 case 0x15: /* SWI333.bi */
259 case 0x1B: /* SWI450 */
272 if (addr_mode == 3) {
273 unaligned_addr = *idx_to_addr(regs, RA3(inst));
274 source_idx = RA3(inst);
276 unaligned_addr = *idx_to_addr(regs, RA5(inst));
277 source_idx = RA5(inst);
281 target_idx = RT3(inst);
283 target_idx = RT4(inst);
286 shift = IMM3U(inst) * len;
289 unaligned_addr += shift;
292 if (!access_ok((void *)unaligned_addr, len))
295 get_data(unaligned_addr, &target_val, len);
296 *idx_to_addr(regs, target_idx) = target_val;
298 if (!access_ok((void *)unaligned_addr, len))
300 target_val = *idx_to_addr(regs, target_idx);
301 set_data((void *)unaligned_addr, target_val, len);
305 *idx_to_addr(regs, source_idx) = unaligned_addr + shift;
313 static inline int do_32(unsigned long inst, struct pt_regs *regs)
315 int imm, regular, load, len, sign_ext;
316 unsigned long unaligned_addr, target_val, shift;
318 unaligned_addr = *idx_to_addr(regs, RA(inst));
320 switch ((inst >> 25) << 1) {
329 case 0x0A: /* LHI.bi */
336 case 0x22: /* LHSI */
343 case 0x2A: /* LHSI.bi */
357 case 0x0C: /* LWI.bi */
371 case 0x1A: /* SHI.bi */
385 case 0x1C: /* SWI.bi */
394 switch (inst & 0xff) {
403 case 0x05: /* LH.bi */
417 case 0x15: /* LHS.bi */
431 case 0x06: /* LW.bi */
445 case 0x0D: /* SH.bi */
459 case 0x0E: /* SW.bi */
473 shift = GET_IMMSVAL(IMM(inst)) * len;
475 shift = *idx_to_addr(regs, RB(inst)) << SV(inst);
478 unaligned_addr += shift;
482 if (!access_ok((void *)unaligned_addr, len))
485 get_data(unaligned_addr, &target_val, len);
488 *idx_to_addr(regs, RT(inst)) =
489 sign_extend(target_val, len);
491 *idx_to_addr(regs, RT(inst)) = target_val;
494 if (!access_ok((void *)unaligned_addr, len))
497 target_val = *idx_to_addr(regs, RT(inst));
498 set_data((void *)unaligned_addr, target_val, len);
502 *idx_to_addr(regs, RA(inst)) = unaligned_addr + shift;
511 int do_unaligned_access(unsigned long addr, struct pt_regs *regs)
517 inst = get_inst(regs->ipc);
519 DEBUG((unalign_access_debug > 0), 1,
520 "Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n", addr,
523 seg = force_uaccess_begin();
524 if (inst & NDS32_16BIT_INSTRUCTION)
525 ret = do_16((inst >> 16) & 0xffff, regs);
527 ret = do_32(inst, regs);
528 force_uaccess_end(seg);
533 #ifdef CONFIG_PROC_FS
535 static struct ctl_table alignment_tbl[3] = {
537 .procname = "enable",
538 .data = &unalign_access_mode,
539 .maxlen = sizeof(unalign_access_mode),
541 .proc_handler = &proc_dointvec
545 .procname = "debug_info",
546 .data = &unalign_access_debug,
547 .maxlen = sizeof(unalign_access_debug),
549 .proc_handler = &proc_dointvec
555 static struct ctl_table nds32_sysctl_table[2] = {
557 .procname = "unaligned_access",
559 .child = alignment_tbl},
563 static struct ctl_path nds32_path[2] = {
564 {.procname = "nds32"},
569 * Initialize nds32 alignment-correction interface
571 static int __init nds32_sysctl_init(void)
573 register_sysctl_paths(nds32_path, nds32_sysctl_table);
577 __initcall(nds32_sysctl_init);
578 #endif /* CONFIG_PROC_FS */