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 | |
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; |
12 | |
13 | static 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 | |
18 | static 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 | */ |
32 | static 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 | */ |
65 | static 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 | */ |
95 | static 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 | */ |
117 | static 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 | */ |
142 | static 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 | */ |
176 | static 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 | |
205 | extern 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 | |
212 | void 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 | |
240 | kernel_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 | |
250 | good_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 | |
287 | bad_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 | |
303 | static 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 | |
334 | static int __init csky_alignment_init(void) |
335 | { |
336 | register_sysctl_init("csky/csky_alignment" , alignment_tbl); |
337 | return 0; |
338 | } |
339 | |
340 | arch_initcall(csky_alignment_init); |
341 | |