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_kern_enable = 1;
9 static int align_usr_enable = 1;
10 static int align_kern_count = 0;
11 static int align_usr_count = 0;
13 static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx)
15 return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx);
18 static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val)
23 *((uint32_t *)&(regs->a0) - 2 + rx) = val;
27 * Get byte-value from addr and set it to *valp.
32 static int ldb_asm(uint32_t addr, uint32_t *valp)
45 ".section __ex_table,\"a\"\n"
50 : "=&r"(err), "=r"(val)
60 * Put byte-value to addr.
65 static int stb_asm(uint32_t addr, uint32_t val)
77 ".section __ex_table,\"a\"\n"
83 : "r"(val), "r" (addr)
90 * Get half-word from [rx + imm]
95 static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
97 uint32_t byte0, byte1;
99 if (ldb_asm(addr, &byte0))
102 if (ldb_asm(addr, &byte1))
106 put_ptreg(regs, rz, byte0);
112 * Store half-word to [rx + imm]
117 static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
119 uint32_t byte0, byte1;
121 byte0 = byte1 = get_ptreg(regs, rz);
125 if (stb_asm(addr, byte0))
129 byte1 = (byte1 >> 8) & 0xff;
130 if (stb_asm(addr, byte1))
137 * Get word from [rx + imm]
142 static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
144 uint32_t byte0, byte1, byte2, byte3;
146 if (ldb_asm(addr, &byte0))
150 if (ldb_asm(addr, &byte1))
154 if (ldb_asm(addr, &byte2))
158 if (ldb_asm(addr, &byte3))
162 byte0 |= byte2 << 16;
163 byte0 |= byte3 << 24;
165 put_ptreg(regs, rz, byte0);
171 * Store word to [rx + imm]
176 static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
178 uint32_t byte0, byte1, byte2, byte3;
180 byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz);
184 if (stb_asm(addr, byte0))
188 byte1 = (byte1 >> 8) & 0xff;
189 if (stb_asm(addr, byte1))
193 byte2 = (byte2 >> 16) & 0xff;
194 if (stb_asm(addr, byte2))
198 byte3 = (byte3 >> 24) & 0xff;
199 if (stb_asm(addr, byte3))
205 extern int fixup_exception(struct pt_regs *regs);
207 #define OP_LDH 0xc000
208 #define OP_STH 0xd000
209 #define OP_LDW 0x8000
210 #define OP_STW 0x9000
212 void csky_alignment(struct pt_regs *regs)
222 if (!user_mode(regs))
225 if (!align_usr_enable) {
226 pr_err("%s user disabled.\n", __func__);
232 ret = get_user(tmp, (uint16_t *)instruction_pointer(regs));
234 pr_err("%s get_user failed.\n", __func__);
241 if (!align_kern_enable) {
242 pr_err("%s kernel disabled.\n", __func__);
248 tmp = *(uint16_t *)instruction_pointer(regs);
251 opcode = (uint32_t)tmp;
254 imm = (opcode >> 4) & 0xf;
255 rz = (opcode >> 8) & 0xf;
258 if (rx == 0 || rx == 1 || rz == 0 || rz == 1)
263 addr = get_ptreg(regs, rx) + (imm << 1);
264 ret = ldh_c(regs, rz, addr);
267 addr = get_ptreg(regs, rx) + (imm << 2);
268 ret = ldw_c(regs, rz, addr);
271 addr = get_ptreg(regs, rx) + (imm << 1);
272 ret = sth_c(regs, rz, addr);
275 addr = get_ptreg(regs, rx) + (imm << 2);
276 ret = stw_c(regs, rz, addr);
288 if (!user_mode(regs)) {
289 if (fixup_exception(regs))
293 pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n",
294 __func__, opcode, rz, rx, imm, addr);
297 make_task_dead(SIGKILL);
300 force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr);
303 static struct ctl_table alignment_tbl[5] = {
305 .procname = "kernel_enable",
306 .data = &align_kern_enable,
307 .maxlen = sizeof(align_kern_enable),
309 .proc_handler = &proc_dointvec
312 .procname = "user_enable",
313 .data = &align_usr_enable,
314 .maxlen = sizeof(align_usr_enable),
316 .proc_handler = &proc_dointvec
319 .procname = "kernel_count",
320 .data = &align_kern_count,
321 .maxlen = sizeof(align_kern_count),
323 .proc_handler = &proc_dointvec
326 .procname = "user_count",
327 .data = &align_usr_count,
328 .maxlen = sizeof(align_usr_count),
330 .proc_handler = &proc_dointvec
335 static struct ctl_table sysctl_table[2] = {
337 .procname = "csky_alignment",
339 .child = alignment_tbl},
343 static struct ctl_path sysctl_path[2] = {
344 {.procname = "csky"},
348 static int __init csky_alignment_init(void)
350 register_sysctl_paths(sysctl_path, sysctl_table);
354 arch_initcall(csky_alignment_init);