Commit | Line | Data |
---|---|---|
d2912cb1 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
56372b0b G |
2 | /* |
3 | * linux/arch/unicore32/mm/alignment.c | |
4 | * | |
5 | * Code specific to PKUnity SoC and UniCore ISA | |
6 | * | |
7 | * Copyright (C) 2001-2010 GUAN Xue-tao | |
56372b0b G |
8 | */ |
9 | /* | |
10 | * TODO: | |
11 | * FPU ldm/stm not handling | |
12 | */ | |
13 | #include <linux/compiler.h> | |
14 | #include <linux/kernel.h> | |
b17b0153 | 15 | #include <linux/sched/debug.h> |
56372b0b G |
16 | #include <linux/errno.h> |
17 | #include <linux/string.h> | |
18 | #include <linux/init.h> | |
19 | #include <linux/sched.h> | |
20 | #include <linux/uaccess.h> | |
ca5999fd | 21 | #include <linux/pgtable.h> |
65fddcfc | 22 | |
56372b0b G |
23 | #include <asm/tlbflush.h> |
24 | #include <asm/unaligned.h> | |
25 | ||
8978bfd2 GX |
26 | #include "mm.h" |
27 | ||
56372b0b G |
28 | #define CODING_BITS(i) (i & 0xe0000120) |
29 | ||
30 | #define LDST_P_BIT(i) (i & (1 << 28)) /* Preindex */ | |
31 | #define LDST_U_BIT(i) (i & (1 << 27)) /* Add offset */ | |
32 | #define LDST_W_BIT(i) (i & (1 << 25)) /* Writeback */ | |
33 | #define LDST_L_BIT(i) (i & (1 << 24)) /* Load */ | |
34 | ||
35 | #define LDST_P_EQ_U(i) ((((i) ^ ((i) >> 1)) & (1 << 27)) == 0) | |
36 | ||
37 | #define LDSTH_I_BIT(i) (i & (1 << 26)) /* half-word immed */ | |
38 | #define LDM_S_BIT(i) (i & (1 << 26)) /* write ASR from BSR */ | |
39 | #define LDM_H_BIT(i) (i & (1 << 6)) /* select r0-r15 or r16-r31 */ | |
40 | ||
41 | #define RN_BITS(i) ((i >> 19) & 31) /* Rn */ | |
42 | #define RD_BITS(i) ((i >> 14) & 31) /* Rd */ | |
43 | #define RM_BITS(i) (i & 31) /* Rm */ | |
44 | ||
45 | #define REGMASK_BITS(i) (((i & 0x7fe00) >> 3) | (i & 0x3f)) | |
46 | #define OFFSET_BITS(i) (i & 0x03fff) | |
47 | ||
48 | #define SHIFT_BITS(i) ((i >> 9) & 0x1f) | |
49 | #define SHIFT_TYPE(i) (i & 0xc0) | |
50 | #define SHIFT_LSL 0x00 | |
51 | #define SHIFT_LSR 0x40 | |
52 | #define SHIFT_ASR 0x80 | |
53 | #define SHIFT_RORRRX 0xc0 | |
54 | ||
55 | union offset_union { | |
56 | unsigned long un; | |
57 | signed long sn; | |
58 | }; | |
59 | ||
60 | #define TYPE_ERROR 0 | |
61 | #define TYPE_FAULT 1 | |
62 | #define TYPE_LDST 2 | |
63 | #define TYPE_DONE 3 | |
64 | #define TYPE_SWAP 4 | |
65 | #define TYPE_COLS 5 /* Coprocessor load/store */ | |
66 | ||
67 | #define get8_unaligned_check(val, addr, err) \ | |
68 | __asm__( \ | |
69 | "1: ldb.u %1, [%2], #1\n" \ | |
70 | "2:\n" \ | |
71 | " .pushsection .fixup,\"ax\"\n" \ | |
72 | " .align 2\n" \ | |
73 | "3: mov %0, #1\n" \ | |
74 | " b 2b\n" \ | |
75 | " .popsection\n" \ | |
76 | " .pushsection __ex_table,\"a\"\n" \ | |
77 | " .align 3\n" \ | |
78 | " .long 1b, 3b\n" \ | |
79 | " .popsection\n" \ | |
80 | : "=r" (err), "=&r" (val), "=r" (addr) \ | |
81 | : "0" (err), "2" (addr)) | |
82 | ||
83 | #define get8t_unaligned_check(val, addr, err) \ | |
84 | __asm__( \ | |
85 | "1: ldb.u %1, [%2], #1\n" \ | |
86 | "2:\n" \ | |
87 | " .pushsection .fixup,\"ax\"\n" \ | |
88 | " .align 2\n" \ | |
89 | "3: mov %0, #1\n" \ | |
90 | " b 2b\n" \ | |
91 | " .popsection\n" \ | |
92 | " .pushsection __ex_table,\"a\"\n" \ | |
93 | " .align 3\n" \ | |
94 | " .long 1b, 3b\n" \ | |
95 | " .popsection\n" \ | |
96 | : "=r" (err), "=&r" (val), "=r" (addr) \ | |
97 | : "0" (err), "2" (addr)) | |
98 | ||
99 | #define get16_unaligned_check(val, addr) \ | |
100 | do { \ | |
101 | unsigned int err = 0, v, a = addr; \ | |
102 | get8_unaligned_check(val, a, err); \ | |
103 | get8_unaligned_check(v, a, err); \ | |
104 | val |= v << 8; \ | |
105 | if (err) \ | |
106 | goto fault; \ | |
107 | } while (0) | |
108 | ||
109 | #define put16_unaligned_check(val, addr) \ | |
110 | do { \ | |
111 | unsigned int err = 0, v = val, a = addr; \ | |
112 | __asm__( \ | |
113 | "1: stb.u %1, [%2], #1\n" \ | |
114 | " mov %1, %1 >> #8\n" \ | |
115 | "2: stb.u %1, [%2]\n" \ | |
116 | "3:\n" \ | |
117 | " .pushsection .fixup,\"ax\"\n" \ | |
118 | " .align 2\n" \ | |
119 | "4: mov %0, #1\n" \ | |
120 | " b 3b\n" \ | |
121 | " .popsection\n" \ | |
122 | " .pushsection __ex_table,\"a\"\n" \ | |
123 | " .align 3\n" \ | |
124 | " .long 1b, 4b\n" \ | |
125 | " .long 2b, 4b\n" \ | |
126 | " .popsection\n" \ | |
127 | : "=r" (err), "=&r" (v), "=&r" (a) \ | |
128 | : "0" (err), "1" (v), "2" (a)); \ | |
129 | if (err) \ | |
130 | goto fault; \ | |
131 | } while (0) | |
132 | ||
133 | #define __put32_unaligned_check(ins, val, addr) \ | |
134 | do { \ | |
135 | unsigned int err = 0, v = val, a = addr; \ | |
136 | __asm__( \ | |
137 | "1: "ins" %1, [%2], #1\n" \ | |
138 | " mov %1, %1 >> #8\n" \ | |
139 | "2: "ins" %1, [%2], #1\n" \ | |
140 | " mov %1, %1 >> #8\n" \ | |
141 | "3: "ins" %1, [%2], #1\n" \ | |
142 | " mov %1, %1 >> #8\n" \ | |
143 | "4: "ins" %1, [%2]\n" \ | |
144 | "5:\n" \ | |
145 | " .pushsection .fixup,\"ax\"\n" \ | |
146 | " .align 2\n" \ | |
147 | "6: mov %0, #1\n" \ | |
148 | " b 5b\n" \ | |
149 | " .popsection\n" \ | |
150 | " .pushsection __ex_table,\"a\"\n" \ | |
151 | " .align 3\n" \ | |
152 | " .long 1b, 6b\n" \ | |
153 | " .long 2b, 6b\n" \ | |
154 | " .long 3b, 6b\n" \ | |
155 | " .long 4b, 6b\n" \ | |
156 | " .popsection\n" \ | |
157 | : "=r" (err), "=&r" (v), "=&r" (a) \ | |
158 | : "0" (err), "1" (v), "2" (a)); \ | |
159 | if (err) \ | |
160 | goto fault; \ | |
161 | } while (0) | |
162 | ||
163 | #define get32_unaligned_check(val, addr) \ | |
164 | do { \ | |
165 | unsigned int err = 0, v, a = addr; \ | |
166 | get8_unaligned_check(val, a, err); \ | |
167 | get8_unaligned_check(v, a, err); \ | |
168 | val |= v << 8; \ | |
169 | get8_unaligned_check(v, a, err); \ | |
170 | val |= v << 16; \ | |
171 | get8_unaligned_check(v, a, err); \ | |
172 | val |= v << 24; \ | |
173 | if (err) \ | |
174 | goto fault; \ | |
175 | } while (0) | |
176 | ||
177 | #define put32_unaligned_check(val, addr) \ | |
178 | __put32_unaligned_check("stb.u", val, addr) | |
179 | ||
180 | #define get32t_unaligned_check(val, addr) \ | |
181 | do { \ | |
182 | unsigned int err = 0, v, a = addr; \ | |
183 | get8t_unaligned_check(val, a, err); \ | |
184 | get8t_unaligned_check(v, a, err); \ | |
185 | val |= v << 8; \ | |
186 | get8t_unaligned_check(v, a, err); \ | |
187 | val |= v << 16; \ | |
188 | get8t_unaligned_check(v, a, err); \ | |
189 | val |= v << 24; \ | |
190 | if (err) \ | |
191 | goto fault; \ | |
192 | } while (0) | |
193 | ||
194 | #define put32t_unaligned_check(val, addr) \ | |
195 | __put32_unaligned_check("stb.u", val, addr) | |
196 | ||
197 | static void | |
198 | do_alignment_finish_ldst(unsigned long addr, unsigned long instr, | |
199 | struct pt_regs *regs, union offset_union offset) | |
200 | { | |
201 | if (!LDST_U_BIT(instr)) | |
202 | offset.un = -offset.un; | |
203 | ||
204 | if (!LDST_P_BIT(instr)) | |
205 | addr += offset.un; | |
206 | ||
207 | if (!LDST_P_BIT(instr) || LDST_W_BIT(instr)) | |
208 | regs->uregs[RN_BITS(instr)] = addr; | |
209 | } | |
210 | ||
211 | static int | |
212 | do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, | |
213 | struct pt_regs *regs) | |
214 | { | |
215 | unsigned int rd = RD_BITS(instr); | |
216 | ||
217 | /* old value 0x40002120, can't judge swap instr correctly */ | |
218 | if ((instr & 0x4b003fe0) == 0x40000120) | |
219 | goto swp; | |
220 | ||
221 | if (LDST_L_BIT(instr)) { | |
222 | unsigned long val; | |
223 | get16_unaligned_check(val, addr); | |
224 | ||
225 | /* signed half-word? */ | |
226 | if (instr & 0x80) | |
227 | val = (signed long)((signed short)val); | |
228 | ||
229 | regs->uregs[rd] = val; | |
230 | } else | |
231 | put16_unaligned_check(regs->uregs[rd], addr); | |
232 | ||
233 | return TYPE_LDST; | |
234 | ||
235 | swp: | |
236 | /* only handle swap word | |
237 | * for swap byte should not active this alignment exception */ | |
238 | get32_unaligned_check(regs->uregs[RD_BITS(instr)], addr); | |
239 | put32_unaligned_check(regs->uregs[RM_BITS(instr)], addr); | |
240 | return TYPE_SWAP; | |
241 | ||
242 | fault: | |
243 | return TYPE_FAULT; | |
244 | } | |
245 | ||
246 | static int | |
247 | do_alignment_ldrstr(unsigned long addr, unsigned long instr, | |
248 | struct pt_regs *regs) | |
249 | { | |
250 | unsigned int rd = RD_BITS(instr); | |
251 | ||
252 | if (!LDST_P_BIT(instr) && LDST_W_BIT(instr)) | |
253 | goto trans; | |
254 | ||
255 | if (LDST_L_BIT(instr)) | |
256 | get32_unaligned_check(regs->uregs[rd], addr); | |
257 | else | |
258 | put32_unaligned_check(regs->uregs[rd], addr); | |
259 | return TYPE_LDST; | |
260 | ||
261 | trans: | |
262 | if (LDST_L_BIT(instr)) | |
263 | get32t_unaligned_check(regs->uregs[rd], addr); | |
264 | else | |
265 | put32t_unaligned_check(regs->uregs[rd], addr); | |
266 | return TYPE_LDST; | |
267 | ||
268 | fault: | |
269 | return TYPE_FAULT; | |
270 | } | |
271 | ||
272 | /* | |
273 | * LDM/STM alignment handler. | |
274 | * | |
275 | * There are 4 variants of this instruction: | |
276 | * | |
277 | * B = rn pointer before instruction, A = rn pointer after instruction | |
278 | * ------ increasing address -----> | |
279 | * | | r0 | r1 | ... | rx | | | |
280 | * PU = 01 B A | |
281 | * PU = 11 B A | |
282 | * PU = 00 A B | |
283 | * PU = 10 A B | |
284 | */ | |
285 | static int | |
286 | do_alignment_ldmstm(unsigned long addr, unsigned long instr, | |
287 | struct pt_regs *regs) | |
288 | { | |
289 | unsigned int rd, rn, pc_correction, reg_correction, nr_regs, regbits; | |
290 | unsigned long eaddr, newaddr; | |
291 | ||
292 | if (LDM_S_BIT(instr)) | |
293 | goto bad; | |
294 | ||
295 | pc_correction = 4; /* processor implementation defined */ | |
296 | ||
297 | /* count the number of registers in the mask to be transferred */ | |
298 | nr_regs = hweight16(REGMASK_BITS(instr)) * 4; | |
299 | ||
300 | rn = RN_BITS(instr); | |
301 | newaddr = eaddr = regs->uregs[rn]; | |
302 | ||
303 | if (!LDST_U_BIT(instr)) | |
304 | nr_regs = -nr_regs; | |
305 | newaddr += nr_regs; | |
306 | if (!LDST_U_BIT(instr)) | |
307 | eaddr = newaddr; | |
308 | ||
309 | if (LDST_P_EQ_U(instr)) /* U = P */ | |
310 | eaddr += 4; | |
311 | ||
312 | /* | |
313 | * This is a "hint" - we already have eaddr worked out by the | |
314 | * processor for us. | |
315 | */ | |
316 | if (addr != eaddr) { | |
317 | printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, " | |
318 | "addr = %08lx, eaddr = %08lx\n", | |
319 | instruction_pointer(regs), instr, addr, eaddr); | |
320 | show_regs(regs); | |
321 | } | |
322 | ||
323 | if (LDM_H_BIT(instr)) | |
324 | reg_correction = 0x10; | |
325 | else | |
326 | reg_correction = 0x00; | |
327 | ||
328 | for (regbits = REGMASK_BITS(instr), rd = 0; regbits; | |
329 | regbits >>= 1, rd += 1) | |
330 | if (regbits & 1) { | |
331 | if (LDST_L_BIT(instr)) | |
332 | get32_unaligned_check(regs-> | |
333 | uregs[rd + reg_correction], eaddr); | |
334 | else | |
335 | put32_unaligned_check(regs-> | |
336 | uregs[rd + reg_correction], eaddr); | |
337 | eaddr += 4; | |
338 | } | |
339 | ||
340 | if (LDST_W_BIT(instr)) | |
341 | regs->uregs[rn] = newaddr; | |
342 | return TYPE_DONE; | |
343 | ||
344 | fault: | |
345 | regs->UCreg_pc -= pc_correction; | |
346 | return TYPE_FAULT; | |
347 | ||
348 | bad: | |
349 | printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n"); | |
350 | return TYPE_ERROR; | |
351 | } | |
352 | ||
353 | static int | |
354 | do_alignment(unsigned long addr, unsigned int error_code, struct pt_regs *regs) | |
355 | { | |
356 | union offset_union offset; | |
357 | unsigned long instr, instrptr; | |
358 | int (*handler) (unsigned long addr, unsigned long instr, | |
359 | struct pt_regs *regs); | |
360 | unsigned int type; | |
361 | ||
362 | instrptr = instruction_pointer(regs); | |
363 | if (instrptr >= PAGE_OFFSET) | |
364 | instr = *(unsigned long *)instrptr; | |
365 | else { | |
366 | __asm__ __volatile__( | |
367 | "ldw.u %0, [%1]\n" | |
368 | : "=&r"(instr) | |
369 | : "r"(instrptr)); | |
370 | } | |
371 | ||
372 | regs->UCreg_pc += 4; | |
373 | ||
374 | switch (CODING_BITS(instr)) { | |
375 | case 0x40000120: /* ldrh or strh */ | |
376 | if (LDSTH_I_BIT(instr)) | |
377 | offset.un = (instr & 0x3e00) >> 4 | (instr & 31); | |
378 | else | |
379 | offset.un = regs->uregs[RM_BITS(instr)]; | |
380 | handler = do_alignment_ldrhstrh; | |
381 | break; | |
382 | ||
383 | case 0x60000000: /* ldr or str immediate */ | |
384 | case 0x60000100: /* ldr or str immediate */ | |
385 | case 0x60000020: /* ldr or str immediate */ | |
386 | case 0x60000120: /* ldr or str immediate */ | |
387 | offset.un = OFFSET_BITS(instr); | |
388 | handler = do_alignment_ldrstr; | |
389 | break; | |
390 | ||
391 | case 0x40000000: /* ldr or str register */ | |
392 | offset.un = regs->uregs[RM_BITS(instr)]; | |
393 | { | |
394 | unsigned int shiftval = SHIFT_BITS(instr); | |
395 | ||
396 | switch (SHIFT_TYPE(instr)) { | |
397 | case SHIFT_LSL: | |
398 | offset.un <<= shiftval; | |
399 | break; | |
400 | ||
401 | case SHIFT_LSR: | |
402 | offset.un >>= shiftval; | |
403 | break; | |
404 | ||
405 | case SHIFT_ASR: | |
406 | offset.sn >>= shiftval; | |
407 | break; | |
408 | ||
409 | case SHIFT_RORRRX: | |
410 | if (shiftval == 0) { | |
411 | offset.un >>= 1; | |
412 | if (regs->UCreg_asr & PSR_C_BIT) | |
413 | offset.un |= 1 << 31; | |
414 | } else | |
415 | offset.un = offset.un >> shiftval | | |
416 | offset.un << (32 - shiftval); | |
417 | break; | |
418 | } | |
419 | } | |
420 | handler = do_alignment_ldrstr; | |
421 | break; | |
422 | ||
423 | case 0x80000000: /* ldm or stm */ | |
424 | case 0x80000020: /* ldm or stm */ | |
425 | handler = do_alignment_ldmstm; | |
426 | break; | |
427 | ||
428 | default: | |
429 | goto bad; | |
430 | } | |
431 | ||
432 | type = handler(addr, instr, regs); | |
433 | ||
434 | if (type == TYPE_ERROR || type == TYPE_FAULT) | |
435 | goto bad_or_fault; | |
436 | ||
437 | if (type == TYPE_LDST) | |
438 | do_alignment_finish_ldst(addr, instr, regs, offset); | |
439 | ||
440 | return 0; | |
441 | ||
442 | bad_or_fault: | |
443 | if (type == TYPE_ERROR) | |
444 | goto bad; | |
445 | regs->UCreg_pc -= 4; | |
446 | /* | |
447 | * We got a fault - fix it up, or die. | |
448 | */ | |
449 | do_bad_area(addr, error_code, regs); | |
450 | return 0; | |
451 | ||
452 | bad: | |
453 | /* | |
454 | * Oops, we didn't handle the instruction. | |
455 | * However, we must handle fpu instr firstly. | |
456 | */ | |
457 | #ifdef CONFIG_UNICORE_FPU_F64 | |
458 | /* handle co.load/store */ | |
459 | #define CODING_COLS 0xc0000000 | |
460 | #define COLS_OFFSET_BITS(i) (i & 0x1FF) | |
461 | #define COLS_L_BITS(i) (i & (1<<24)) | |
462 | #define COLS_FN_BITS(i) ((i>>14) & 31) | |
463 | if ((instr & 0xe0000000) == CODING_COLS) { | |
464 | unsigned int fn = COLS_FN_BITS(instr); | |
465 | unsigned long val = 0; | |
466 | if (COLS_L_BITS(instr)) { | |
467 | get32t_unaligned_check(val, addr); | |
468 | switch (fn) { | |
469 | #define ASM_MTF(n) case n: \ | |
470 | __asm__ __volatile__("MTF %0, F" __stringify(n) \ | |
471 | : : "r"(val)); \ | |
472 | break; | |
473 | ASM_MTF(0); ASM_MTF(1); ASM_MTF(2); ASM_MTF(3); | |
474 | ASM_MTF(4); ASM_MTF(5); ASM_MTF(6); ASM_MTF(7); | |
475 | ASM_MTF(8); ASM_MTF(9); ASM_MTF(10); ASM_MTF(11); | |
476 | ASM_MTF(12); ASM_MTF(13); ASM_MTF(14); ASM_MTF(15); | |
477 | ASM_MTF(16); ASM_MTF(17); ASM_MTF(18); ASM_MTF(19); | |
478 | ASM_MTF(20); ASM_MTF(21); ASM_MTF(22); ASM_MTF(23); | |
479 | ASM_MTF(24); ASM_MTF(25); ASM_MTF(26); ASM_MTF(27); | |
480 | ASM_MTF(28); ASM_MTF(29); ASM_MTF(30); ASM_MTF(31); | |
481 | #undef ASM_MTF | |
482 | } | |
483 | } else { | |
484 | switch (fn) { | |
485 | #define ASM_MFF(n) case n: \ | |
486 | __asm__ __volatile__("MFF %0, F" __stringify(n) \ | |
487 | : : "r"(val)); \ | |
488 | break; | |
489 | ASM_MFF(0); ASM_MFF(1); ASM_MFF(2); ASM_MFF(3); | |
490 | ASM_MFF(4); ASM_MFF(5); ASM_MFF(6); ASM_MFF(7); | |
491 | ASM_MFF(8); ASM_MFF(9); ASM_MFF(10); ASM_MFF(11); | |
492 | ASM_MFF(12); ASM_MFF(13); ASM_MFF(14); ASM_MFF(15); | |
493 | ASM_MFF(16); ASM_MFF(17); ASM_MFF(18); ASM_MFF(19); | |
494 | ASM_MFF(20); ASM_MFF(21); ASM_MFF(22); ASM_MFF(23); | |
495 | ASM_MFF(24); ASM_MFF(25); ASM_MFF(26); ASM_MFF(27); | |
496 | ASM_MFF(28); ASM_MFF(29); ASM_MFF(30); ASM_MFF(31); | |
497 | #undef ASM_MFF | |
498 | } | |
499 | put32t_unaligned_check(val, addr); | |
500 | } | |
501 | return TYPE_COLS; | |
502 | } | |
503 | fault: | |
504 | return TYPE_FAULT; | |
505 | #endif | |
506 | printk(KERN_ERR "Alignment trap: not handling instruction " | |
507 | "%08lx at [<%08lx>]\n", instr, instrptr); | |
508 | return 1; | |
509 | } | |
510 | ||
511 | /* | |
512 | * This needs to be done after sysctl_init, otherwise sys/ will be | |
513 | * overwritten. Actually, this shouldn't be in sys/ at all since | |
514 | * it isn't a sysctl, and it doesn't contain sysctl information. | |
515 | */ | |
516 | static int __init alignment_init(void) | |
517 | { | |
518 | hook_fault_code(1, do_alignment, SIGBUS, BUS_ADRALN, | |
519 | "alignment exception"); | |
520 | ||
521 | return 0; | |
522 | } | |
523 | ||
524 | fs_initcall(alignment_init); |