1 // SPDX-License-Identifier: GPL-2.0
2 // Copyright (C) 2005-2017 Andes Technology Corporation
3
4 #include <linux/proc_fs.h>
5 #include <linux/uaccess.h>
6 #include <linux/sysctl.h>
7 #include <asm/unaligned.h>
8
9 #define DEBUG(enable, tagged, ...) \
10 do{ \
11 if (enable) { \
12 if (tagged) \
13 pr_warn("[ %30s() ] ", __func__); \
14 pr_warn(__VA_ARGS__); \
15 } \
16 } while (0)
17
18 #define RT(inst) (((inst) >> 20) & 0x1FUL)
19 #define RA(inst) (((inst) >> 15) & 0x1FUL)
20 #define RB(inst) (((inst) >> 10) & 0x1FUL)
21 #define SV(inst) (((inst) >> 8) & 0x3UL)
22 #define IMM(inst) (((inst) >> 0) & 0x7FFFUL)
23
24 #define RA3(inst) (((inst) >> 3) & 0x7UL)
25 #define RT3(inst) (((inst) >> 6) & 0x7UL)
26 #define IMM3U(inst) (((inst) >> 0) & 0x7UL)
27
28 #define RA5(inst) (((inst) >> 0) & 0x1FUL)
29 #define RT4(inst) (((inst) >> 5) & 0xFUL)
30
31 #define GET_IMMSVAL(imm_value) \
32 (((imm_value >> 14) & 0x1) ? (imm_value - 0x8000) : imm_value)
33
34 #define __get8_data(val,addr,err) \
35 __asm__( \
36 "1: lbi.bi %1, [%2], #1\n" \
37 "2:\n" \
38 " .pushsection .text.fixup,\"ax\"\n" \
39 " .align 2\n" \
40 "3: movi %0, #1\n" \
41 " j 2b\n" \
42 " .popsection\n" \
43 " .pushsection __ex_table,\"a\"\n" \
44 " .align 3\n" \
45 " .long 1b, 3b\n" \
46 " .popsection\n" \
47 : "=r" (err), "=&r" (val), "=r" (addr) \
48 : "0" (err), "2" (addr))
49
50 #define get16_data(addr, val_ptr) \
51 do { \
52 unsigned int err = 0, v, a = addr; \
53 __get8_data(v,a,err); \
54 *val_ptr = v << 0; \
55 __get8_data(v,a,err); \
56 *val_ptr |= v << 8; \
57 if (err) \
58 goto fault; \
59 *val_ptr = le16_to_cpu(*val_ptr); \
60 } while(0)
61
62 #define get32_data(addr, val_ptr) \
63 do { \
64 unsigned int err = 0, v, a = addr; \
65 __get8_data(v,a,err); \
66 *val_ptr = v << 0; \
67 __get8_data(v,a,err); \
68 *val_ptr |= v << 8; \
69 __get8_data(v,a,err); \
70 *val_ptr |= v << 16; \
71 __get8_data(v,a,err); \
72 *val_ptr |= v << 24; \
73 if (err) \
74 goto fault; \
75 *val_ptr = le32_to_cpu(*val_ptr); \
76 } while(0)
77
78 #define get_data(addr, val_ptr, len) \
79 if (len == 2) \
80 get16_data(addr, val_ptr); \
81 else \
82 get32_data(addr, val_ptr);
83
84 #define set16_data(addr, val) \
85 do { \
86 unsigned int err = 0, *ptr = addr ; \
87 val = le32_to_cpu(val); \
88 __asm__( \
89 "1: sbi.bi %2, [%1], #1\n" \
90 " srli %2, %2, #8\n" \
91 "2: sbi %2, [%1]\n" \
92 "3:\n" \
93 " .pushsection .text.fixup,\"ax\"\n" \
94 " .align 2\n" \
95 "4: movi %0, #1\n" \
96 " j 3b\n" \
97 " .popsection\n" \
98 " .pushsection __ex_table,\"a\"\n" \
99 " .align 3\n" \
100 " .long 1b, 4b\n" \
101 " .long 2b, 4b\n" \
102 " .popsection\n" \
103 : "=r" (err), "+r" (ptr), "+r" (val) \
104 : "0" (err) \
105 ); \
106 if (err) \
107 goto fault; \
108 } while(0)
109
110 #define set32_data(addr, val) \
111 do { \
112 unsigned int err = 0, *ptr = addr ; \
113 val = le32_to_cpu(val); \
114 __asm__( \
115 "1: sbi.bi %2, [%1], #1\n" \
116 " srli %2, %2, #8\n" \
117 "2: sbi.bi %2, [%1], #1\n" \
118 " srli %2, %2, #8\n" \
119 "3: sbi.bi %2, [%1], #1\n" \
120 " srli %2, %2, #8\n" \
121 "4: sbi %2, [%1]\n" \
122 "5:\n" \
123 " .pushsection .text.fixup,\"ax\"\n" \
124 " .align 2\n" \
125 "6: movi %0, #1\n" \
126 " j 5b\n" \
127 " .popsection\n" \
128 " .pushsection __ex_table,\"a\"\n" \
129 " .align 3\n" \
130 " .long 1b, 6b\n" \
131 " .long 2b, 6b\n" \
132 " .long 3b, 6b\n" \
133 " .long 4b, 6b\n" \
134 " .popsection\n" \
135 : "=r" (err), "+r" (ptr), "+r" (val) \
136 : "0" (err) \
137 ); \
138 if (err) \
139 goto fault; \
140 } while(0)
141 #define set_data(addr, val, len) \
142 if (len == 2) \
143 set16_data(addr, val); \
144 else \
145 set32_data(addr, val);
146 #define NDS32_16BIT_INSTRUCTION 0x80000000
147
148 extern pte_t va_present(struct mm_struct *mm, unsigned long addr);
149 extern pte_t va_kernel_present(unsigned long addr);
150 extern int va_readable(struct pt_regs *regs, unsigned long addr);
151 extern int va_writable(struct pt_regs *regs, unsigned long addr);
152
153 int unalign_access_mode = 0, unalign_access_debug = 0;
154
idx_to_addr(struct pt_regs * regs,int idx)155 static inline unsigned long *idx_to_addr(struct pt_regs *regs, int idx)
156 {
157 /* this should be consistent with ptrace.h */
158 if (idx >= 0 && idx <= 25) /* R0-R25 */
159 return ®s->uregs[0] + idx;
160 else if (idx >= 28 && idx <= 30) /* FP, GP, LP */
161 return ®s->fp + (idx - 28);
162 else if (idx == 31) /* SP */
163 return ®s->sp;
164 else
165 return NULL; /* cause a segfault */
166 }
167
get_inst(unsigned long addr)168 static inline unsigned long get_inst(unsigned long addr)
169 {
170 return be32_to_cpu(get_unaligned((u32 *) addr));
171 }
172
sign_extend(unsigned long val,int len)173 static inline unsigned long sign_extend(unsigned long val, int len)
174 {
175 unsigned long ret = 0;
176 unsigned char *s, *t;
177 int i = 0;
178
179 val = cpu_to_le32(val);
180
181 s = (void *)&val;
182 t = (void *)&ret;
183
184 while (i++ < len)
185 *t++ = *s++;
186
187 if (((*(t - 1)) & 0x80) && (i < 4)) {
188
189 while (i++ <= 4)
190 *t++ = 0xff;
191 }
192
193 return le32_to_cpu(ret);
194 }
195
do_16(unsigned long inst,struct pt_regs * regs)196 static inline int do_16(unsigned long inst, struct pt_regs *regs)
197 {
198 int imm, regular, load, len, addr_mode, idx_mode;
199 unsigned long unaligned_addr, target_val, source_idx, target_idx,
200 shift = 0;
201 switch ((inst >> 9) & 0x3F) {
202
203 case 0x12: /* LHI333 */
204 imm = 1;
205 regular = 1;
206 load = 1;
207 len = 2;
208 addr_mode = 3;
209 idx_mode = 3;
210 break;
211 case 0x10: /* LWI333 */
212 imm = 1;
213 regular = 1;
214 load = 1;
215 len = 4;
216 addr_mode = 3;
217 idx_mode = 3;
218 break;
219 case 0x11: /* LWI333.bi */
220 imm = 1;
221 regular = 0;
222 load = 1;
223 len = 4;
224 addr_mode = 3;
225 idx_mode = 3;
226 break;
227 case 0x1A: /* LWI450 */
228 imm = 0;
229 regular = 1;
230 load = 1;
231 len = 4;
232 addr_mode = 5;
233 idx_mode = 4;
234 break;
235 case 0x16: /* SHI333 */
236 imm = 1;
237 regular = 1;
238 load = 0;
239 len = 2;
240 addr_mode = 3;
241 idx_mode = 3;
242 break;
243 case 0x14: /* SWI333 */
244 imm = 1;
245 regular = 1;
246 load = 0;
247 len = 4;
248 addr_mode = 3;
249 idx_mode = 3;
250 break;
251 case 0x15: /* SWI333.bi */
252 imm = 1;
253 regular = 0;
254 load = 0;
255 len = 4;
256 addr_mode = 3;
257 idx_mode = 3;
258 break;
259 case 0x1B: /* SWI450 */
260 imm = 0;
261 regular = 1;
262 load = 0;
263 len = 4;
264 addr_mode = 5;
265 idx_mode = 4;
266 break;
267
268 default:
269 return -EFAULT;
270 }
271
272 if (addr_mode == 3) {
273 unaligned_addr = *idx_to_addr(regs, RA3(inst));
274 source_idx = RA3(inst);
275 } else {
276 unaligned_addr = *idx_to_addr(regs, RA5(inst));
277 source_idx = RA5(inst);
278 }
279
280 if (idx_mode == 3)
281 target_idx = RT3(inst);
282 else
283 target_idx = RT4(inst);
284
285 if (imm)
286 shift = IMM3U(inst) * len;
287
288 if (regular)
289 unaligned_addr += shift;
290
291 if (load) {
292 if (!access_ok((void *)unaligned_addr, len))
293 return -EACCES;
294
295 get_data(unaligned_addr, &target_val, len);
296 *idx_to_addr(regs, target_idx) = target_val;
297 } else {
298 if (!access_ok((void *)unaligned_addr, len))
299 return -EACCES;
300 target_val = *idx_to_addr(regs, target_idx);
301 set_data((void *)unaligned_addr, target_val, len);
302 }
303
304 if (!regular)
305 *idx_to_addr(regs, source_idx) = unaligned_addr + shift;
306 regs->ipc += 2;
307
308 return 0;
309 fault:
310 return -EACCES;
311 }
312
do_32(unsigned long inst,struct pt_regs * regs)313 static inline int do_32(unsigned long inst, struct pt_regs *regs)
314 {
315 int imm, regular, load, len, sign_ext;
316 unsigned long unaligned_addr, target_val, shift;
317
318 unaligned_addr = *idx_to_addr(regs, RA(inst));
319
320 switch ((inst >> 25) << 1) {
321
322 case 0x02: /* LHI */
323 imm = 1;
324 regular = 1;
325 load = 1;
326 len = 2;
327 sign_ext = 0;
328 break;
329 case 0x0A: /* LHI.bi */
330 imm = 1;
331 regular = 0;
332 load = 1;
333 len = 2;
334 sign_ext = 0;
335 break;
336 case 0x22: /* LHSI */
337 imm = 1;
338 regular = 1;
339 load = 1;
340 len = 2;
341 sign_ext = 1;
342 break;
343 case 0x2A: /* LHSI.bi */
344 imm = 1;
345 regular = 0;
346 load = 1;
347 len = 2;
348 sign_ext = 1;
349 break;
350 case 0x04: /* LWI */
351 imm = 1;
352 regular = 1;
353 load = 1;
354 len = 4;
355 sign_ext = 0;
356 break;
357 case 0x0C: /* LWI.bi */
358 imm = 1;
359 regular = 0;
360 load = 1;
361 len = 4;
362 sign_ext = 0;
363 break;
364 case 0x12: /* SHI */
365 imm = 1;
366 regular = 1;
367 load = 0;
368 len = 2;
369 sign_ext = 0;
370 break;
371 case 0x1A: /* SHI.bi */
372 imm = 1;
373 regular = 0;
374 load = 0;
375 len = 2;
376 sign_ext = 0;
377 break;
378 case 0x14: /* SWI */
379 imm = 1;
380 regular = 1;
381 load = 0;
382 len = 4;
383 sign_ext = 0;
384 break;
385 case 0x1C: /* SWI.bi */
386 imm = 1;
387 regular = 0;
388 load = 0;
389 len = 4;
390 sign_ext = 0;
391 break;
392
393 default:
394 switch (inst & 0xff) {
395
396 case 0x01: /* LH */
397 imm = 0;
398 regular = 1;
399 load = 1;
400 len = 2;
401 sign_ext = 0;
402 break;
403 case 0x05: /* LH.bi */
404 imm = 0;
405 regular = 0;
406 load = 1;
407 len = 2;
408 sign_ext = 0;
409 break;
410 case 0x11: /* LHS */
411 imm = 0;
412 regular = 1;
413 load = 1;
414 len = 2;
415 sign_ext = 1;
416 break;
417 case 0x15: /* LHS.bi */
418 imm = 0;
419 regular = 0;
420 load = 1;
421 len = 2;
422 sign_ext = 1;
423 break;
424 case 0x02: /* LW */
425 imm = 0;
426 regular = 1;
427 load = 1;
428 len = 4;
429 sign_ext = 0;
430 break;
431 case 0x06: /* LW.bi */
432 imm = 0;
433 regular = 0;
434 load = 1;
435 len = 4;
436 sign_ext = 0;
437 break;
438 case 0x09: /* SH */
439 imm = 0;
440 regular = 1;
441 load = 0;
442 len = 2;
443 sign_ext = 0;
444 break;
445 case 0x0D: /* SH.bi */
446 imm = 0;
447 regular = 0;
448 load = 0;
449 len = 2;
450 sign_ext = 0;
451 break;
452 case 0x0A: /* SW */
453 imm = 0;
454 regular = 1;
455 load = 0;
456 len = 4;
457 sign_ext = 0;
458 break;
459 case 0x0E: /* SW.bi */
460 imm = 0;
461 regular = 0;
462 load = 0;
463 len = 4;
464 sign_ext = 0;
465 break;
466
467 default:
468 return -EFAULT;
469 }
470 }
471
472 if (imm)
473 shift = GET_IMMSVAL(IMM(inst)) * len;
474 else
475 shift = *idx_to_addr(regs, RB(inst)) << SV(inst);
476
477 if (regular)
478 unaligned_addr += shift;
479
480 if (load) {
481
482 if (!access_ok((void *)unaligned_addr, len))
483 return -EACCES;
484
485 get_data(unaligned_addr, &target_val, len);
486
487 if (sign_ext)
488 *idx_to_addr(regs, RT(inst)) =
489 sign_extend(target_val, len);
490 else
491 *idx_to_addr(regs, RT(inst)) = target_val;
492 } else {
493
494 if (!access_ok((void *)unaligned_addr, len))
495 return -EACCES;
496
497 target_val = *idx_to_addr(regs, RT(inst));
498 set_data((void *)unaligned_addr, target_val, len);
499 }
500
501 if (!regular)
502 *idx_to_addr(regs, RA(inst)) = unaligned_addr + shift;
503
504 regs->ipc += 4;
505
506 return 0;
507 fault:
508 return -EACCES;
509 }
510
do_unaligned_access(unsigned long addr,struct pt_regs * regs)511 int do_unaligned_access(unsigned long addr, struct pt_regs *regs)
512 {
513 unsigned long inst;
514 int ret = -EFAULT;
515 mm_segment_t seg = get_fs();
516
517 inst = get_inst(regs->ipc);
518
519 DEBUG((unalign_access_debug > 0), 1,
520 "Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n", addr,
521 regs->ipc, inst);
522
523 set_fs(USER_DS);
524
525 if (inst & NDS32_16BIT_INSTRUCTION)
526 ret = do_16((inst >> 16) & 0xffff, regs);
527 else
528 ret = do_32(inst, regs);
529 set_fs(seg);
530
531 return ret;
532 }
533
534 #ifdef CONFIG_PROC_FS
535
536 static struct ctl_table alignment_tbl[3] = {
537 {
538 .procname = "enable",
539 .data = &unalign_access_mode,
540 .maxlen = sizeof(unalign_access_mode),
541 .mode = 0666,
542 .proc_handler = &proc_dointvec
543 }
544 ,
545 {
546 .procname = "debug_info",
547 .data = &unalign_access_debug,
548 .maxlen = sizeof(unalign_access_debug),
549 .mode = 0644,
550 .proc_handler = &proc_dointvec
551 }
552 ,
553 {}
554 };
555
556 static struct ctl_table nds32_sysctl_table[2] = {
557 {
558 .procname = "unaligned_access",
559 .mode = 0555,
560 .child = alignment_tbl},
561 {}
562 };
563
564 static struct ctl_path nds32_path[2] = {
565 {.procname = "nds32"},
566 {}
567 };
568
569 /*
570 * Initialize nds32 alignment-correction interface
571 */
nds32_sysctl_init(void)572 static int __init nds32_sysctl_init(void)
573 {
574 register_sysctl_paths(nds32_path, nds32_sysctl_table);
575 return 0;
576 }
577
578 __initcall(nds32_sysctl_init);
579 #endif /* CONFIG_PROC_FS */
580