diff options
Diffstat (limited to 'arch/nds32/mm/alignment.c')
| -rw-r--r-- | arch/nds32/mm/alignment.c | 576 |
1 files changed, 576 insertions, 0 deletions
diff --git a/arch/nds32/mm/alignment.c b/arch/nds32/mm/alignment.c new file mode 100644 index 000000000000..b96a01b10ca7 --- /dev/null +++ b/arch/nds32/mm/alignment.c @@ -0,0 +1,576 @@ +// SPDX-License-Identifier: GPL-2.0 +// Copyright (C) 2005-2017 Andes Technology Corporation + +#include <linux/proc_fs.h> +#include <linux/uaccess.h> +#include <linux/sysctl.h> +#include <asm/unaligned.h> + +#define DEBUG(enable, tagged, ...) \ + do{ \ + if (enable) { \ + if (tagged) \ + pr_warn("[ %30s() ] ", __func__); \ + pr_warn(__VA_ARGS__); \ + } \ + } while (0) + +#define RT(inst) (((inst) >> 20) & 0x1FUL) +#define RA(inst) (((inst) >> 15) & 0x1FUL) +#define RB(inst) (((inst) >> 10) & 0x1FUL) +#define SV(inst) (((inst) >> 8) & 0x3UL) +#define IMM(inst) (((inst) >> 0) & 0x3FFFUL) + +#define RA3(inst) (((inst) >> 3) & 0x7UL) +#define RT3(inst) (((inst) >> 6) & 0x7UL) +#define IMM3U(inst) (((inst) >> 0) & 0x7UL) + +#define RA5(inst) (((inst) >> 0) & 0x1FUL) +#define RT4(inst) (((inst) >> 5) & 0xFUL) + +#define __get8_data(val,addr,err) \ + __asm__( \ + "1: lbi.bi %1, [%2], #1\n" \ + "2:\n" \ + " .pushsection .text.fixup,\"ax\"\n" \ + " .align 2\n" \ + "3: movi %0, #1\n" \ + " j 2b\n" \ + " .popsection\n" \ + " .pushsection __ex_table,\"a\"\n" \ + " .align 3\n" \ + " .long 1b, 3b\n" \ + " .popsection\n" \ + : "=r" (err), "=&r" (val), "=r" (addr) \ + : "0" (err), "2" (addr)) + +#define get16_data(addr, val_ptr) \ + do { \ + unsigned int err = 0, v, a = addr; \ + __get8_data(v,a,err); \ + *val_ptr = v << 0; \ + __get8_data(v,a,err); \ + *val_ptr |= v << 8; \ + if (err) \ + goto fault; \ + *val_ptr = le16_to_cpu(*val_ptr); \ + } while(0) + +#define get32_data(addr, val_ptr) \ + do { \ + unsigned int err = 0, v, a = addr; \ + __get8_data(v,a,err); \ + *val_ptr = v << 0; \ + __get8_data(v,a,err); \ + *val_ptr |= v << 8; \ + __get8_data(v,a,err); \ + *val_ptr |= v << 16; \ + __get8_data(v,a,err); \ + *val_ptr |= v << 24; \ + if (err) \ + goto fault; \ + *val_ptr = le32_to_cpu(*val_ptr); \ + } while(0) + +#define get_data(addr, val_ptr, len) \ + if (len == 2) \ + get16_data(addr, val_ptr); \ + else \ + get32_data(addr, val_ptr); + +#define set16_data(addr, val) \ + do { \ + unsigned int err = 0, *ptr = addr ; \ + val = le32_to_cpu(val); \ + __asm__( \ + "1: sbi.bi %2, [%1], #1\n" \ + " srli %2, %2, #8\n" \ + "2: sbi %2, [%1]\n" \ + "3:\n" \ + " .pushsection .text.fixup,\"ax\"\n" \ + " .align 2\n" \ + "4: movi %0, #1\n" \ + " j 3b\n" \ + " .popsection\n" \ + " .pushsection __ex_table,\"a\"\n" \ + " .align 3\n" \ + " .long 1b, 4b\n" \ + " .long 2b, 4b\n" \ + " .popsection\n" \ + : "=r" (err), "+r" (ptr), "+r" (val) \ + : "0" (err) \ + ); \ + if (err) \ + goto fault; \ + } while(0) + +#define set32_data(addr, val) \ + do { \ + unsigned int err = 0, *ptr = addr ; \ + val = le32_to_cpu(val); \ + __asm__( \ + "1: sbi.bi %2, [%1], #1\n" \ + " srli %2, %2, #8\n" \ + "2: sbi.bi %2, [%1], #1\n" \ + " srli %2, %2, #8\n" \ + "3: sbi.bi %2, [%1], #1\n" \ + " srli %2, %2, #8\n" \ + "4: sbi %2, [%1]\n" \ + "5:\n" \ + " .pushsection .text.fixup,\"ax\"\n" \ + " .align 2\n" \ + "6: movi %0, #1\n" \ + " j 5b\n" \ + " .popsection\n" \ + " .pushsection __ex_table,\"a\"\n" \ + " .align 3\n" \ + " .long 1b, 6b\n" \ + " .long 2b, 6b\n" \ + " .long 3b, 6b\n" \ + " .long 4b, 6b\n" \ + " .popsection\n" \ + : "=r" (err), "+r" (ptr), "+r" (val) \ + : "0" (err) \ + ); \ + if (err) \ + goto fault; \ + } while(0) +#define set_data(addr, val, len) \ + if (len == 2) \ + set16_data(addr, val); \ + else \ + set32_data(addr, val); +#define NDS32_16BIT_INSTRUCTION 0x80000000 + +extern pte_t va_present(struct mm_struct *mm, unsigned long addr); +extern pte_t va_kernel_present(unsigned long addr); +extern int va_readable(struct pt_regs *regs, unsigned long addr); +extern int va_writable(struct pt_regs *regs, unsigned long addr); + +int unalign_access_mode = 0, unalign_access_debug = 0; + +static inline unsigned long *idx_to_addr(struct pt_regs *regs, int idx) +{ + /* this should be consistent with ptrace.h */ + if (idx >= 0 && idx <= 25) /* R0-R25 */ + return ®s->uregs[0] + idx; + else if (idx >= 28 && idx <= 30) /* FP, GP, LP */ + return ®s->fp + (idx - 28); + else if (idx == 31) /* SP */ + return ®s->sp; + else + return NULL; /* cause a segfault */ +} + +static inline unsigned long get_inst(unsigned long addr) +{ + return be32_to_cpu(get_unaligned((u32 *) addr)); +} + +static inline unsigned long sign_extend(unsigned long val, int len) +{ + unsigned long ret = 0; + unsigned char *s, *t; + int i = 0; + + val = cpu_to_le32(val); + + s = (void *)&val; + t = (void *)&ret; + + while (i++ < len) + *t++ = *s++; + + if (((*(t - 1)) & 0x80) && (i < 4)) { + + while (i++ <= 4) + *t++ = 0xff; + } + + return le32_to_cpu(ret); +} + +static inline int do_16(unsigned long inst, struct pt_regs *regs) +{ + int imm, regular, load, len, addr_mode, idx_mode; + unsigned long unaligned_addr, target_val, source_idx, target_idx, + shift = 0; + switch ((inst >> 9) & 0x3F) { + + case 0x12: /* LHI333 */ + imm = 1; + regular = 1; + load = 1; + len = 2; + addr_mode = 3; + idx_mode = 3; + break; + case 0x10: /* LWI333 */ + imm = 1; + regular = 1; + load = 1; + len = 4; + addr_mode = 3; + idx_mode = 3; + break; + case 0x11: /* LWI333.bi */ + imm = 1; + regular = 0; + load = 1; + len = 4; + addr_mode = 3; + idx_mode = 3; + break; + case 0x1A: /* LWI450 */ + imm = 0; + regular = 1; + load = 1; + len = 4; + addr_mode = 5; + idx_mode = 4; + break; + case 0x16: /* SHI333 */ + imm = 1; + regular = 1; + load = 0; + len = 2; + addr_mode = 3; + idx_mode = 3; + break; + case 0x14: /* SWI333 */ + imm = 1; + regular = 1; + load = 0; + len = 4; + addr_mode = 3; + idx_mode = 3; + break; + case 0x15: /* SWI333.bi */ + imm = 1; + regular = 0; + load = 0; + len = 4; + addr_mode = 3; + idx_mode = 3; + break; + case 0x1B: /* SWI450 */ + imm = 0; + regular = 1; + load = 0; + len = 4; + addr_mode = 5; + idx_mode = 4; + break; + + default: + return -EFAULT; + } + + if (addr_mode == 3) { + unaligned_addr = *idx_to_addr(regs, RA3(inst)); + source_idx = RA3(inst); + } else { + unaligned_addr = *idx_to_addr(regs, RA5(inst)); + source_idx = RA5(inst); + } + + if (idx_mode == 3) + target_idx = RT3(inst); + else + target_idx = RT4(inst); + + if (imm) + shift = IMM3U(inst) * len; + + if (regular) + unaligned_addr += shift; + + if (load) { + if (!access_ok(VERIFY_READ, (void *)unaligned_addr, len)) + return -EACCES; + + get_data(unaligned_addr, &target_val, len); + *idx_to_addr(regs, target_idx) = target_val; + } else { + if (!access_ok(VERIFY_WRITE, (void *)unaligned_addr, len)) + return -EACCES; + target_val = *idx_to_addr(regs, target_idx); + set_data((void *)unaligned_addr, target_val, len); + } + + if (!regular) + *idx_to_addr(regs, source_idx) = unaligned_addr + shift; + regs->ipc += 2; + + return 0; +fault: + return -EACCES; +} + +static inline int do_32(unsigned long inst, struct pt_regs *regs) +{ + int imm, regular, load, len, sign_ext; + unsigned long unaligned_addr, target_val, shift; + + unaligned_addr = *idx_to_addr(regs, RA(inst)); + + switch ((inst >> 25) << 1) { + + case 0x02: /* LHI */ + imm = 1; + regular = 1; + load = 1; + len = 2; + sign_ext = 0; + break; + case 0x0A: /* LHI.bi */ + imm = 1; + regular = 0; + load = 1; + len = 2; + sign_ext = 0; + break; + case 0x22: /* LHSI */ + imm = 1; + regular = 1; + load = 1; + len = 2; + sign_ext = 1; + break; + case 0x2A: /* LHSI.bi */ + imm = 1; + regular = 0; + load = 1; + len = 2; + sign_ext = 1; + break; + case 0x04: /* LWI */ + imm = 1; + regular = 1; + load = 1; + len = 4; + sign_ext = 0; + break; + case 0x0C: /* LWI.bi */ + imm = 1; + regular = 0; + load = 1; + len = 4; + sign_ext = 0; + break; + case 0x12: /* SHI */ + imm = 1; + regular = 1; + load = 0; + len = 2; + sign_ext = 0; + break; + case 0x1A: /* SHI.bi */ + imm = 1; + regular = 0; + load = 0; + len = 2; + sign_ext = 0; + break; + case 0x14: /* SWI */ + imm = 1; + regular = 1; + load = 0; + len = 4; + sign_ext = 0; + break; + case 0x1C: /* SWI.bi */ + imm = 1; + regular = 0; + load = 0; + len = 4; + sign_ext = 0; + break; + + default: + switch (inst & 0xff) { + + case 0x01: /* LH */ + imm = 0; + regular = 1; + load = 1; + len = 2; + sign_ext = 0; + break; + case 0x05: /* LH.bi */ + imm = 0; + regular = 0; + load = 1; + len = 2; + sign_ext = 0; + break; + case 0x11: /* LHS */ + imm = 0; + regular = 1; + load = 1; + len = 2; + sign_ext = 1; + break; + case 0x15: /* LHS.bi */ + imm = 0; + regular = 0; + load = 1; + len = 2; + sign_ext = 1; + break; + case 0x02: /* LW */ + imm = 0; + regular = 1; + load = 1; + len = 4; + sign_ext = 0; + break; + case 0x06: /* LW.bi */ + imm = 0; + regular = 0; + load = 1; + len = 4; + sign_ext = 0; + break; + case 0x09: /* SH */ + imm = 0; + regular = 1; + load = 0; + len = 2; + sign_ext = 0; + break; + case 0x0D: /* SH.bi */ + imm = 0; + regular = 0; + load = 0; + len = 2; + sign_ext = 0; + break; + case 0x0A: /* SW */ + imm = 0; + regular = 1; + load = 0; + len = 4; + sign_ext = 0; + break; + case 0x0E: /* SW.bi */ + imm = 0; + regular = 0; + load = 0; + len = 4; + sign_ext = 0; + break; + + default: + return -EFAULT; + } + } + + if (imm) + shift = IMM(inst) * len; + else + shift = *idx_to_addr(regs, RB(inst)) << SV(inst); + + if (regular) + unaligned_addr += shift; + + if (load) { + + if (!access_ok(VERIFY_READ, (void *)unaligned_addr, len)) + return -EACCES; + + get_data(unaligned_addr, &target_val, len); + + if (sign_ext) + *idx_to_addr(regs, RT(inst)) = + sign_extend(target_val, len); + else + *idx_to_addr(regs, RT(inst)) = target_val; + } else { + + if (!access_ok(VERIFY_WRITE, (void *)unaligned_addr, len)) + return -EACCES; + + target_val = *idx_to_addr(regs, RT(inst)); + set_data((void *)unaligned_addr, target_val, len); + } + + if (!regular) + *idx_to_addr(regs, RA(inst)) = unaligned_addr + shift; + + regs->ipc += 4; + + return 0; +fault: + return -EACCES; +} + +int do_unaligned_access(unsigned long addr, struct pt_regs *regs) +{ + unsigned long inst; + int ret = -EFAULT; + mm_segment_t seg = get_fs(); + + inst = get_inst(regs->ipc); + + DEBUG((unalign_access_debug > 0), 1, + "Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n", addr, + regs->ipc, inst); + + set_fs(USER_DS); + + if (inst & NDS32_16BIT_INSTRUCTION) + ret = do_16((inst >> 16) & 0xffff, regs); + else + ret = do_32(inst, regs); + set_fs(seg); + + return ret; +} + +#ifdef CONFIG_PROC_FS + +static struct ctl_table alignment_tbl[3] = { + { + .procname = "enable", + .data = &unalign_access_mode, + .maxlen = sizeof(unalign_access_mode), + .mode = 0666, + .proc_handler = &proc_dointvec + } + , + { + .procname = "debug_info", + .data = &unalign_access_debug, + .maxlen = sizeof(unalign_access_debug), + .mode = 0644, + .proc_handler = &proc_dointvec + } + , + {} +}; + +static struct ctl_table nds32_sysctl_table[2] = { + { + .procname = "unaligned_acess", + .mode = 0555, + .child = alignment_tbl}, + {} +}; + +static struct ctl_path nds32_path[2] = { + {.procname = "nds32"}, + {} +}; + +/* + * Initialize nds32 alignment-correction interface + */ +static int __init nds32_sysctl_init(void) +{ + register_sysctl_paths(nds32_path, nds32_sysctl_table); + return 0; +} + +__initcall(nds32_sysctl_init); +#endif /* CONFIG_PROC_FS */ |