1 // SPDX-License-Identifier: GPL-2.0
2 // Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
4 #include <linux/kernel.h>
5 #include <linux/uaccess.h>
6 #include <linux/ptrace.h>
8 static int align_enable = 1;
9 static int align_count;
11 static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx)
13 return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx);
16 static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val)
21 *((uint32_t *)&(regs->a0) - 2 + rx) = val;
25 * Get byte-value from addr and set it to *valp.
30 static int ldb_asm(uint32_t addr, uint32_t *valp)
35 if (!access_ok((void *)addr, 1))
46 ".section __ex_table,\"a\"\n"
51 : "=&r"(err), "=r"(val)
61 * Put byte-value to addr.
66 static int stb_asm(uint32_t addr, uint32_t val)
70 if (!access_ok((void *)addr, 1))
81 ".section __ex_table,\"a\"\n"
87 : "r"(val), "r" (addr)
94 * Get half-word from [rx + imm]
99 static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
101 uint32_t byte0, byte1;
103 if (ldb_asm(addr, &byte0))
106 if (ldb_asm(addr, &byte1))
110 put_ptreg(regs, rz, byte0);
116 * Store half-word to [rx + imm]
121 static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
123 uint32_t byte0, byte1;
125 byte0 = byte1 = get_ptreg(regs, rz);
129 if (stb_asm(addr, byte0))
133 byte1 = (byte1 >> 8) & 0xff;
134 if (stb_asm(addr, byte1))
141 * Get word from [rx + imm]
146 static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
148 uint32_t byte0, byte1, byte2, byte3;
150 if (ldb_asm(addr, &byte0))
154 if (ldb_asm(addr, &byte1))
158 if (ldb_asm(addr, &byte2))
162 if (ldb_asm(addr, &byte3))
166 byte0 |= byte2 << 16;
167 byte0 |= byte3 << 24;
169 put_ptreg(regs, rz, byte0);
175 * Store word to [rx + imm]
180 static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
182 uint32_t byte0, byte1, byte2, byte3;
184 byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz);
188 if (stb_asm(addr, byte0))
192 byte1 = (byte1 >> 8) & 0xff;
193 if (stb_asm(addr, byte1))
197 byte2 = (byte2 >> 16) & 0xff;
198 if (stb_asm(addr, byte2))
202 byte3 = (byte3 >> 24) & 0xff;
203 if (stb_asm(addr, byte3))
211 extern int fixup_exception(struct pt_regs *regs);
213 #define OP_LDH 0xc000
214 #define OP_STH 0xd000
215 #define OP_LDW 0x8000
216 #define OP_STW 0x9000
218 void csky_alignment(struct pt_regs *regs)
228 if (!user_mode(regs))
231 ret = get_user(tmp, (uint16_t *)instruction_pointer(regs));
233 pr_err("%s get_user failed.\n", __func__);
237 opcode = (uint32_t)tmp;
240 imm = (opcode >> 4) & 0xf;
241 rz = (opcode >> 8) & 0xf;
244 if (rx == 0 || rx == 1 || rz == 0 || rz == 1)
249 addr = get_ptreg(regs, rx) + (imm << 1);
250 ret = ldh_c(regs, rz, addr);
253 addr = get_ptreg(regs, rx) + (imm << 2);
254 ret = ldw_c(regs, rz, addr);
257 addr = get_ptreg(regs, rx) + (imm << 1);
258 ret = sth_c(regs, rz, addr);
261 addr = get_ptreg(regs, rx) + (imm << 2);
262 ret = stw_c(regs, rz, addr);
274 if (!user_mode(regs)) {
275 if (fixup_exception(regs))
279 pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n",
280 __func__, opcode, rz, rx, imm, addr);
286 force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr, current);
289 static struct ctl_table alignment_tbl[4] = {
291 .procname = "enable",
292 .data = &align_enable,
293 .maxlen = sizeof(align_enable),
295 .proc_handler = &proc_dointvec
299 .data = &align_count,
300 .maxlen = sizeof(align_count),
302 .proc_handler = &proc_dointvec
307 static struct ctl_table sysctl_table[2] = {
309 .procname = "csky_alignment",
311 .child = alignment_tbl},
315 static struct ctl_path sysctl_path[2] = {
316 {.procname = "csky"},
320 static int __init csky_alignment_init(void)
322 register_sysctl_paths(sysctl_path, sysctl_table);
326 arch_initcall(csky_alignment_init);