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
get_ptreg(struct pt_regs * regs,uint32_t rx)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
put_ptreg(struct pt_regs * regs,uint32_t rx,uint32_t val)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 */
ldb_asm(uint32_t addr,uint32_t * valp)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 */
stb_asm(uint32_t addr,uint32_t val)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 */
ldh_c(struct pt_regs * regs,uint32_t rz,uint32_t addr)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, &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 */
sth_c(struct pt_regs * regs,uint32_t rz,uint32_t addr)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, 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 */
ldw_c(struct pt_regs * regs,uint32_t rz,uint32_t addr)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, &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 */
stw_c(struct pt_regs * regs,uint32_t rz,uint32_t addr)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, 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
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
csky_alignment(struct pt_regs * regs)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(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
303 static const struct ctl_table alignment_tbl[] = {
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
csky_alignment_init(void)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