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, valp: &byte0))
100 return 1;
101 addr += 1;
102 if (ldb_asm(addr, valp: &byte1))
103 return 1;
104
105 byte0 |= byte1 << 8;
106 put_ptreg(regs, rx: rz, val: 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, rx: rz);
122
123 byte0 &= 0xff;
124
125 if (stb_asm(addr, val: byte0))
126 return 1;
127
128 addr += 1;
129 byte1 = (byte1 >> 8) & 0xff;
130 if (stb_asm(addr, val: 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, valp: &byte0))
147 return 1;
148
149 addr += 1;
150 if (ldb_asm(addr, valp: &byte1))
151 return 1;
152
153 addr += 1;
154 if (ldb_asm(addr, valp: &byte2))
155 return 1;
156
157 addr += 1;
158 if (ldb_asm(addr, valp: &byte3))
159 return 1;
160
161 byte0 |= byte1 << 8;
162 byte0 |= byte2 << 16;
163 byte0 |= byte3 << 24;
164
165 put_ptreg(regs, rx: rz, val: 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, rx: rz);
181
182 byte0 &= 0xff;
183
184 if (stb_asm(addr, val: byte0))
185 return 1;
186
187 addr += 1;
188 byte1 = (byte1 >> 8) & 0xff;
189 if (stb_asm(addr, val: byte1))
190 return 1;
191
192 addr += 1;
193 byte2 = (byte2 >> 16) & 0xff;
194 if (stb_asm(addr, val: byte2))
195 return 1;
196
197 addr += 1;
198 byte3 = (byte3 >> 24) & 0xff;
199 if (stb_asm(addr, val: 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(yes: 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(yes: 0);
297 make_task_dead(SIGKILL);
298 }
299
300 force_sig_fault(SIGBUS, BUS_ADRALN, addr: (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

source code of linux/arch/csky/abiv1/alignment.c