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