1// SPDX-License-Identifier: GPL-2.0
2// Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
3
4#include <linux/kernel.h>
5#include <linux/uaccess.h>
6#include <linux/ptrace.h>
7
8static int align_kern_enable = 1;
9static int align_usr_enable = 1;
10static int align_kern_count = 0;
11static int align_usr_count = 0;
12
13static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx)
14{
15	return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx);
16}
17
18static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val)
19{
20	if (rx == 15)
21		regs->lr = val;
22	else
23		*((uint32_t *)&(regs->a0) - 2 + rx) = val;
24}
25
26/*
27 * Get byte-value from addr and set it to *valp.
28 *
29 * Success: return 0
30 * Failure: return 1
31 */
32static int ldb_asm(uint32_t addr, uint32_t *valp)
33{
34	uint32_t val;
35	int err;
36
37	asm volatile (
38		"movi	%0, 0\n"
39		"1:\n"
40		"ldb	%1, (%2)\n"
41		"br	3f\n"
42		"2:\n"
43		"movi	%0, 1\n"
44		"br	3f\n"
45		".section __ex_table,\"a\"\n"
46		".align 2\n"
47		".long	1b, 2b\n"
48		".previous\n"
49		"3:\n"
50		: "=&r"(err), "=r"(val)
51		: "r" (addr)
52	);
53
54	*valp = val;
55
56	return err;
57}
58
59/*
60 * Put byte-value to addr.
61 *
62 * Success: return 0
63 * Failure: return 1
64 */
65static int stb_asm(uint32_t addr, uint32_t val)
66{
67	int err;
68
69	asm volatile (
70		"movi	%0, 0\n"
71		"1:\n"
72		"stb	%1, (%2)\n"
73		"br	3f\n"
74		"2:\n"
75		"movi	%0, 1\n"
76		"br	3f\n"
77		".section __ex_table,\"a\"\n"
78		".align 2\n"
79		".long	1b, 2b\n"
80		".previous\n"
81		"3:\n"
82		: "=&r"(err)
83		: "r"(val), "r" (addr)
84	);
85
86	return err;
87}
88
89/*
90 * Get half-word from [rx + imm]
91 *
92 * Success: return 0
93 * Failure: return 1
94 */
95static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
96{
97	uint32_t byte0, byte1;
98
99	if (ldb_asm(addr, &byte0))
100		return 1;
101	addr += 1;
102	if (ldb_asm(addr, &byte1))
103		return 1;
104
105	byte0 |= byte1 << 8;
106	put_ptreg(regs, rz, byte0);
107
108	return 0;
109}
110
111/*
112 * Store half-word to [rx + imm]
113 *
114 * Success: return 0
115 * Failure: return 1
116 */
117static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
118{
119	uint32_t byte0, byte1;
120
121	byte0 = byte1 = get_ptreg(regs, rz);
122
123	byte0 &= 0xff;
124
125	if (stb_asm(addr, byte0))
126		return 1;
127
128	addr += 1;
129	byte1 = (byte1 >> 8) & 0xff;
130	if (stb_asm(addr, byte1))
131		return 1;
132
133	return 0;
134}
135
136/*
137 * Get word from [rx + imm]
138 *
139 * Success: return 0
140 * Failure: return 1
141 */
142static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
143{
144	uint32_t byte0, byte1, byte2, byte3;
145
146	if (ldb_asm(addr, &byte0))
147		return 1;
148
149	addr += 1;
150	if (ldb_asm(addr, &byte1))
151		return 1;
152
153	addr += 1;
154	if (ldb_asm(addr, &byte2))
155		return 1;
156
157	addr += 1;
158	if (ldb_asm(addr, &byte3))
159		return 1;
160
161	byte0 |= byte1 << 8;
162	byte0 |= byte2 << 16;
163	byte0 |= byte3 << 24;
164
165	put_ptreg(regs, rz, byte0);
166
167	return 0;
168}
169
170/*
171 * Store word to [rx + imm]
172 *
173 * Success: return 0
174 * Failure: return 1
175 */
176static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr)
177{
178	uint32_t byte0, byte1, byte2, byte3;
179
180	byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz);
181
182	byte0 &= 0xff;
183
184	if (stb_asm(addr, byte0))
185		return 1;
186
187	addr += 1;
188	byte1 = (byte1 >> 8) & 0xff;
189	if (stb_asm(addr, byte1))
190		return 1;
191
192	addr += 1;
193	byte2 = (byte2 >> 16) & 0xff;
194	if (stb_asm(addr, byte2))
195		return 1;
196
197	addr += 1;
198	byte3 = (byte3 >> 24) & 0xff;
199	if (stb_asm(addr, byte3))
200		return 1;
201
202	return 0;
203}
204
205extern int fixup_exception(struct pt_regs *regs);
206
207#define OP_LDH 0xc000
208#define OP_STH 0xd000
209#define OP_LDW 0x8000
210#define OP_STW 0x9000
211
212void csky_alignment(struct pt_regs *regs)
213{
214	int ret;
215	uint16_t tmp;
216	uint32_t opcode = 0;
217	uint32_t rx     = 0;
218	uint32_t rz     = 0;
219	uint32_t imm    = 0;
220	uint32_t addr   = 0;
221
222	if (!user_mode(regs))
223		goto kernel_area;
224
225	if (!align_usr_enable) {
226		pr_err("%s user disabled.\n", __func__);
227		goto bad_area;
228	}
229
230	align_usr_count++;
231
232	ret = get_user(tmp, (uint16_t *)instruction_pointer(regs));
233	if (ret) {
234		pr_err("%s get_user failed.\n", __func__);
235		goto bad_area;
236	}
237
238	goto good_area;
239
240kernel_area:
241	if (!align_kern_enable) {
242		pr_err("%s kernel disabled.\n", __func__);
243		goto bad_area;
244	}
245
246	align_kern_count++;
247
248	tmp = *(uint16_t *)instruction_pointer(regs);
249
250good_area:
251	opcode = (uint32_t)tmp;
252
253	rx  = opcode & 0xf;
254	imm = (opcode >> 4) & 0xf;
255	rz  = (opcode >> 8) & 0xf;
256	opcode &= 0xf000;
257
258	if (rx == 0 || rx == 1 || rz == 0 || rz == 1)
259		goto bad_area;
260
261	switch (opcode) {
262	case OP_LDH:
263		addr = get_ptreg(regs, rx) + (imm << 1);
264		ret = ldh_c(regs, rz, addr);
265		break;
266	case OP_LDW:
267		addr = get_ptreg(regs, rx) + (imm << 2);
268		ret = ldw_c(regs, rz, addr);
269		break;
270	case OP_STH:
271		addr = get_ptreg(regs, rx) + (imm << 1);
272		ret = sth_c(regs, rz, addr);
273		break;
274	case OP_STW:
275		addr = get_ptreg(regs, rx) + (imm << 2);
276		ret = stw_c(regs, rz, addr);
277		break;
278	}
279
280	if (ret)
281		goto bad_area;
282
283	regs->pc += 2;
284
285	return;
286
287bad_area:
288	if (!user_mode(regs)) {
289		if (fixup_exception(regs))
290			return;
291
292		bust_spinlocks(1);
293		pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n",
294				__func__, opcode, rz, rx, imm, addr);
295		show_regs(regs);
296		bust_spinlocks(0);
297		make_task_dead(SIGKILL);
298	}
299
300	force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr);
301}
302
303static struct ctl_table alignment_tbl[5] = {
304	{
305		.procname = "kernel_enable",
306		.data = &align_kern_enable,
307		.maxlen = sizeof(align_kern_enable),
308		.mode = 0666,
309		.proc_handler = &proc_dointvec
310	},
311	{
312		.procname = "user_enable",
313		.data = &align_usr_enable,
314		.maxlen = sizeof(align_usr_enable),
315		.mode = 0666,
316		.proc_handler = &proc_dointvec
317	},
318	{
319		.procname = "kernel_count",
320		.data = &align_kern_count,
321		.maxlen = sizeof(align_kern_count),
322		.mode = 0666,
323		.proc_handler = &proc_dointvec
324	},
325	{
326		.procname = "user_count",
327		.data = &align_usr_count,
328		.maxlen = sizeof(align_usr_count),
329		.mode = 0666,
330		.proc_handler = &proc_dointvec
331	},
332};
333
334static int __init csky_alignment_init(void)
335{
336	register_sysctl_init("csky/csky_alignment", alignment_tbl);
337	return 0;
338}
339
340arch_initcall(csky_alignment_init);
341