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