xref: /openbmc/linux/arch/arm/mm/alignment.c (revision e23feb16)
1 /*
2  *  linux/arch/arm/mm/alignment.c
3  *
4  *  Copyright (C) 1995  Linus Torvalds
5  *  Modifications for ARM processor (c) 1995-2001 Russell King
6  *  Thumb alignment fault fixups (c) 2004 MontaVista Software, Inc.
7  *  - Adapted from gdb/sim/arm/thumbemu.c -- Thumb instruction emulation.
8  *    Copyright (C) 1996, Cygnus Software Technologies Ltd.
9  *
10  * This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License version 2 as
12  * published by the Free Software Foundation.
13  */
14 #include <linux/moduleparam.h>
15 #include <linux/compiler.h>
16 #include <linux/kernel.h>
17 #include <linux/errno.h>
18 #include <linux/string.h>
19 #include <linux/proc_fs.h>
20 #include <linux/seq_file.h>
21 #include <linux/init.h>
22 #include <linux/sched.h>
23 #include <linux/uaccess.h>
24 
25 #include <asm/cp15.h>
26 #include <asm/system_info.h>
27 #include <asm/unaligned.h>
28 
29 #include "fault.h"
30 
31 /*
32  * 32-bit misaligned trap handler (c) 1998 San Mehat (CCC) -July 1998
33  * /proc/sys/debug/alignment, modified and integrated into
34  * Linux 2.1 by Russell King
35  *
36  * Speed optimisations and better fault handling by Russell King.
37  *
38  * *** NOTE ***
39  * This code is not portable to processors with late data abort handling.
40  */
41 #define CODING_BITS(i)	(i & 0x0e000000)
42 
43 #define LDST_I_BIT(i)	(i & (1 << 26))		/* Immediate constant	*/
44 #define LDST_P_BIT(i)	(i & (1 << 24))		/* Preindex		*/
45 #define LDST_U_BIT(i)	(i & (1 << 23))		/* Add offset		*/
46 #define LDST_W_BIT(i)	(i & (1 << 21))		/* Writeback		*/
47 #define LDST_L_BIT(i)	(i & (1 << 20))		/* Load			*/
48 
49 #define LDST_P_EQ_U(i)	((((i) ^ ((i) >> 1)) & (1 << 23)) == 0)
50 
51 #define LDSTHD_I_BIT(i)	(i & (1 << 22))		/* double/half-word immed */
52 #define LDM_S_BIT(i)	(i & (1 << 22))		/* write CPSR from SPSR	*/
53 
54 #define RN_BITS(i)	((i >> 16) & 15)	/* Rn			*/
55 #define RD_BITS(i)	((i >> 12) & 15)	/* Rd			*/
56 #define RM_BITS(i)	(i & 15)		/* Rm			*/
57 
58 #define REGMASK_BITS(i)	(i & 0xffff)
59 #define OFFSET_BITS(i)	(i & 0x0fff)
60 
61 #define IS_SHIFT(i)	(i & 0x0ff0)
62 #define SHIFT_BITS(i)	((i >> 7) & 0x1f)
63 #define SHIFT_TYPE(i)	(i & 0x60)
64 #define SHIFT_LSL	0x00
65 #define SHIFT_LSR	0x20
66 #define SHIFT_ASR	0x40
67 #define SHIFT_RORRRX	0x60
68 
69 #define BAD_INSTR 	0xdeadc0de
70 
71 /* Thumb-2 32 bit format per ARMv7 DDI0406A A6.3, either f800h,e800h,f800h */
72 #define IS_T32(hi16) \
73 	(((hi16) & 0xe000) == 0xe000 && ((hi16) & 0x1800))
74 
75 static unsigned long ai_user;
76 static unsigned long ai_sys;
77 static unsigned long ai_skipped;
78 static unsigned long ai_half;
79 static unsigned long ai_word;
80 static unsigned long ai_dword;
81 static unsigned long ai_multi;
82 static int ai_usermode;
83 
84 core_param(alignment, ai_usermode, int, 0600);
85 
86 #define UM_WARN		(1 << 0)
87 #define UM_FIXUP	(1 << 1)
88 #define UM_SIGNAL	(1 << 2)
89 
90 /* Return true if and only if the ARMv6 unaligned access model is in use. */
91 static bool cpu_is_v6_unaligned(void)
92 {
93 	return cpu_architecture() >= CPU_ARCH_ARMv6 && (cr_alignment & CR_U);
94 }
95 
96 static int safe_usermode(int new_usermode, bool warn)
97 {
98 	/*
99 	 * ARMv6 and later CPUs can perform unaligned accesses for
100 	 * most single load and store instructions up to word size.
101 	 * LDM, STM, LDRD and STRD still need to be handled.
102 	 *
103 	 * Ignoring the alignment fault is not an option on these
104 	 * CPUs since we spin re-faulting the instruction without
105 	 * making any progress.
106 	 */
107 	if (cpu_is_v6_unaligned() && !(new_usermode & (UM_FIXUP | UM_SIGNAL))) {
108 		new_usermode |= UM_FIXUP;
109 
110 		if (warn)
111 			printk(KERN_WARNING "alignment: ignoring faults is unsafe on this CPU.  Defaulting to fixup mode.\n");
112 	}
113 
114 	return new_usermode;
115 }
116 
117 #ifdef CONFIG_PROC_FS
118 static const char *usermode_action[] = {
119 	"ignored",
120 	"warn",
121 	"fixup",
122 	"fixup+warn",
123 	"signal",
124 	"signal+warn"
125 };
126 
127 static int alignment_proc_show(struct seq_file *m, void *v)
128 {
129 	seq_printf(m, "User:\t\t%lu\n", ai_user);
130 	seq_printf(m, "System:\t\t%lu\n", ai_sys);
131 	seq_printf(m, "Skipped:\t%lu\n", ai_skipped);
132 	seq_printf(m, "Half:\t\t%lu\n", ai_half);
133 	seq_printf(m, "Word:\t\t%lu\n", ai_word);
134 	if (cpu_architecture() >= CPU_ARCH_ARMv5TE)
135 		seq_printf(m, "DWord:\t\t%lu\n", ai_dword);
136 	seq_printf(m, "Multi:\t\t%lu\n", ai_multi);
137 	seq_printf(m, "User faults:\t%i (%s)\n", ai_usermode,
138 			usermode_action[ai_usermode]);
139 
140 	return 0;
141 }
142 
143 static int alignment_proc_open(struct inode *inode, struct file *file)
144 {
145 	return single_open(file, alignment_proc_show, NULL);
146 }
147 
148 static ssize_t alignment_proc_write(struct file *file, const char __user *buffer,
149 				    size_t count, loff_t *pos)
150 {
151 	char mode;
152 
153 	if (count > 0) {
154 		if (get_user(mode, buffer))
155 			return -EFAULT;
156 		if (mode >= '0' && mode <= '5')
157 			ai_usermode = safe_usermode(mode - '0', true);
158 	}
159 	return count;
160 }
161 
162 static const struct file_operations alignment_proc_fops = {
163 	.open		= alignment_proc_open,
164 	.read		= seq_read,
165 	.llseek		= seq_lseek,
166 	.release	= single_release,
167 	.write		= alignment_proc_write,
168 };
169 #endif /* CONFIG_PROC_FS */
170 
171 union offset_union {
172 	unsigned long un;
173 	  signed long sn;
174 };
175 
176 #define TYPE_ERROR	0
177 #define TYPE_FAULT	1
178 #define TYPE_LDST	2
179 #define TYPE_DONE	3
180 
181 #ifdef __ARMEB__
182 #define BE		1
183 #define FIRST_BYTE_16	"mov	%1, %1, ror #8\n"
184 #define FIRST_BYTE_32	"mov	%1, %1, ror #24\n"
185 #define NEXT_BYTE	"ror #24"
186 #else
187 #define BE		0
188 #define FIRST_BYTE_16
189 #define FIRST_BYTE_32
190 #define NEXT_BYTE	"lsr #8"
191 #endif
192 
193 #define __get8_unaligned_check(ins,val,addr,err)	\
194 	__asm__(					\
195  ARM(	"1:	"ins"	%1, [%2], #1\n"	)		\
196  THUMB(	"1:	"ins"	%1, [%2]\n"	)		\
197  THUMB(	"	add	%2, %2, #1\n"	)		\
198 	"2:\n"						\
199 	"	.pushsection .fixup,\"ax\"\n"		\
200 	"	.align	2\n"				\
201 	"3:	mov	%0, #1\n"			\
202 	"	b	2b\n"				\
203 	"	.popsection\n"				\
204 	"	.pushsection __ex_table,\"a\"\n"	\
205 	"	.align	3\n"				\
206 	"	.long	1b, 3b\n"			\
207 	"	.popsection\n"				\
208 	: "=r" (err), "=&r" (val), "=r" (addr)		\
209 	: "0" (err), "2" (addr))
210 
211 #define __get16_unaligned_check(ins,val,addr)			\
212 	do {							\
213 		unsigned int err = 0, v, a = addr;		\
214 		__get8_unaligned_check(ins,v,a,err);		\
215 		val =  v << ((BE) ? 8 : 0);			\
216 		__get8_unaligned_check(ins,v,a,err);		\
217 		val |= v << ((BE) ? 0 : 8);			\
218 		if (err)					\
219 			goto fault;				\
220 	} while (0)
221 
222 #define get16_unaligned_check(val,addr) \
223 	__get16_unaligned_check("ldrb",val,addr)
224 
225 #define get16t_unaligned_check(val,addr) \
226 	__get16_unaligned_check("ldrbt",val,addr)
227 
228 #define __get32_unaligned_check(ins,val,addr)			\
229 	do {							\
230 		unsigned int err = 0, v, a = addr;		\
231 		__get8_unaligned_check(ins,v,a,err);		\
232 		val =  v << ((BE) ? 24 :  0);			\
233 		__get8_unaligned_check(ins,v,a,err);		\
234 		val |= v << ((BE) ? 16 :  8);			\
235 		__get8_unaligned_check(ins,v,a,err);		\
236 		val |= v << ((BE) ?  8 : 16);			\
237 		__get8_unaligned_check(ins,v,a,err);		\
238 		val |= v << ((BE) ?  0 : 24);			\
239 		if (err)					\
240 			goto fault;				\
241 	} while (0)
242 
243 #define get32_unaligned_check(val,addr) \
244 	__get32_unaligned_check("ldrb",val,addr)
245 
246 #define get32t_unaligned_check(val,addr) \
247 	__get32_unaligned_check("ldrbt",val,addr)
248 
249 #define __put16_unaligned_check(ins,val,addr)			\
250 	do {							\
251 		unsigned int err = 0, v = val, a = addr;	\
252 		__asm__( FIRST_BYTE_16				\
253 	 ARM(	"1:	"ins"	%1, [%2], #1\n"	)		\
254 	 THUMB(	"1:	"ins"	%1, [%2]\n"	)		\
255 	 THUMB(	"	add	%2, %2, #1\n"	)		\
256 		"	mov	%1, %1, "NEXT_BYTE"\n"		\
257 		"2:	"ins"	%1, [%2]\n"			\
258 		"3:\n"						\
259 		"	.pushsection .fixup,\"ax\"\n"		\
260 		"	.align	2\n"				\
261 		"4:	mov	%0, #1\n"			\
262 		"	b	3b\n"				\
263 		"	.popsection\n"				\
264 		"	.pushsection __ex_table,\"a\"\n"	\
265 		"	.align	3\n"				\
266 		"	.long	1b, 4b\n"			\
267 		"	.long	2b, 4b\n"			\
268 		"	.popsection\n"				\
269 		: "=r" (err), "=&r" (v), "=&r" (a)		\
270 		: "0" (err), "1" (v), "2" (a));			\
271 		if (err)					\
272 			goto fault;				\
273 	} while (0)
274 
275 #define put16_unaligned_check(val,addr)  \
276 	__put16_unaligned_check("strb",val,addr)
277 
278 #define put16t_unaligned_check(val,addr) \
279 	__put16_unaligned_check("strbt",val,addr)
280 
281 #define __put32_unaligned_check(ins,val,addr)			\
282 	do {							\
283 		unsigned int err = 0, v = val, a = addr;	\
284 		__asm__( FIRST_BYTE_32				\
285 	 ARM(	"1:	"ins"	%1, [%2], #1\n"	)		\
286 	 THUMB(	"1:	"ins"	%1, [%2]\n"	)		\
287 	 THUMB(	"	add	%2, %2, #1\n"	)		\
288 		"	mov	%1, %1, "NEXT_BYTE"\n"		\
289 	 ARM(	"2:	"ins"	%1, [%2], #1\n"	)		\
290 	 THUMB(	"2:	"ins"	%1, [%2]\n"	)		\
291 	 THUMB(	"	add	%2, %2, #1\n"	)		\
292 		"	mov	%1, %1, "NEXT_BYTE"\n"		\
293 	 ARM(	"3:	"ins"	%1, [%2], #1\n"	)		\
294 	 THUMB(	"3:	"ins"	%1, [%2]\n"	)		\
295 	 THUMB(	"	add	%2, %2, #1\n"	)		\
296 		"	mov	%1, %1, "NEXT_BYTE"\n"		\
297 		"4:	"ins"	%1, [%2]\n"			\
298 		"5:\n"						\
299 		"	.pushsection .fixup,\"ax\"\n"		\
300 		"	.align	2\n"				\
301 		"6:	mov	%0, #1\n"			\
302 		"	b	5b\n"				\
303 		"	.popsection\n"				\
304 		"	.pushsection __ex_table,\"a\"\n"	\
305 		"	.align	3\n"				\
306 		"	.long	1b, 6b\n"			\
307 		"	.long	2b, 6b\n"			\
308 		"	.long	3b, 6b\n"			\
309 		"	.long	4b, 6b\n"			\
310 		"	.popsection\n"				\
311 		: "=r" (err), "=&r" (v), "=&r" (a)		\
312 		: "0" (err), "1" (v), "2" (a));			\
313 		if (err)					\
314 			goto fault;				\
315 	} while (0)
316 
317 #define put32_unaligned_check(val,addr) \
318 	__put32_unaligned_check("strb", val, addr)
319 
320 #define put32t_unaligned_check(val,addr) \
321 	__put32_unaligned_check("strbt", val, addr)
322 
323 static void
324 do_alignment_finish_ldst(unsigned long addr, unsigned long instr, struct pt_regs *regs, union offset_union offset)
325 {
326 	if (!LDST_U_BIT(instr))
327 		offset.un = -offset.un;
328 
329 	if (!LDST_P_BIT(instr))
330 		addr += offset.un;
331 
332 	if (!LDST_P_BIT(instr) || LDST_W_BIT(instr))
333 		regs->uregs[RN_BITS(instr)] = addr;
334 }
335 
336 static int
337 do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, struct pt_regs *regs)
338 {
339 	unsigned int rd = RD_BITS(instr);
340 
341 	ai_half += 1;
342 
343 	if (user_mode(regs))
344 		goto user;
345 
346 	if (LDST_L_BIT(instr)) {
347 		unsigned long val;
348 		get16_unaligned_check(val, addr);
349 
350 		/* signed half-word? */
351 		if (instr & 0x40)
352 			val = (signed long)((signed short) val);
353 
354 		regs->uregs[rd] = val;
355 	} else
356 		put16_unaligned_check(regs->uregs[rd], addr);
357 
358 	return TYPE_LDST;
359 
360  user:
361 	if (LDST_L_BIT(instr)) {
362 		unsigned long val;
363 		get16t_unaligned_check(val, addr);
364 
365 		/* signed half-word? */
366 		if (instr & 0x40)
367 			val = (signed long)((signed short) val);
368 
369 		regs->uregs[rd] = val;
370 	} else
371 		put16t_unaligned_check(regs->uregs[rd], addr);
372 
373 	return TYPE_LDST;
374 
375  fault:
376 	return TYPE_FAULT;
377 }
378 
379 static int
380 do_alignment_ldrdstrd(unsigned long addr, unsigned long instr,
381 		      struct pt_regs *regs)
382 {
383 	unsigned int rd = RD_BITS(instr);
384 	unsigned int rd2;
385 	int load;
386 
387 	if ((instr & 0xfe000000) == 0xe8000000) {
388 		/* ARMv7 Thumb-2 32-bit LDRD/STRD */
389 		rd2 = (instr >> 8) & 0xf;
390 		load = !!(LDST_L_BIT(instr));
391 	} else if (((rd & 1) == 1) || (rd == 14))
392 		goto bad;
393 	else {
394 		load = ((instr & 0xf0) == 0xd0);
395 		rd2 = rd + 1;
396 	}
397 
398 	ai_dword += 1;
399 
400 	if (user_mode(regs))
401 		goto user;
402 
403 	if (load) {
404 		unsigned long val;
405 		get32_unaligned_check(val, addr);
406 		regs->uregs[rd] = val;
407 		get32_unaligned_check(val, addr + 4);
408 		regs->uregs[rd2] = val;
409 	} else {
410 		put32_unaligned_check(regs->uregs[rd], addr);
411 		put32_unaligned_check(regs->uregs[rd2], addr + 4);
412 	}
413 
414 	return TYPE_LDST;
415 
416  user:
417 	if (load) {
418 		unsigned long val;
419 		get32t_unaligned_check(val, addr);
420 		regs->uregs[rd] = val;
421 		get32t_unaligned_check(val, addr + 4);
422 		regs->uregs[rd2] = val;
423 	} else {
424 		put32t_unaligned_check(regs->uregs[rd], addr);
425 		put32t_unaligned_check(regs->uregs[rd2], addr + 4);
426 	}
427 
428 	return TYPE_LDST;
429  bad:
430 	return TYPE_ERROR;
431  fault:
432 	return TYPE_FAULT;
433 }
434 
435 static int
436 do_alignment_ldrstr(unsigned long addr, unsigned long instr, struct pt_regs *regs)
437 {
438 	unsigned int rd = RD_BITS(instr);
439 
440 	ai_word += 1;
441 
442 	if ((!LDST_P_BIT(instr) && LDST_W_BIT(instr)) || user_mode(regs))
443 		goto trans;
444 
445 	if (LDST_L_BIT(instr)) {
446 		unsigned int val;
447 		get32_unaligned_check(val, addr);
448 		regs->uregs[rd] = val;
449 	} else
450 		put32_unaligned_check(regs->uregs[rd], addr);
451 	return TYPE_LDST;
452 
453  trans:
454 	if (LDST_L_BIT(instr)) {
455 		unsigned int val;
456 		get32t_unaligned_check(val, addr);
457 		regs->uregs[rd] = val;
458 	} else
459 		put32t_unaligned_check(regs->uregs[rd], addr);
460 	return TYPE_LDST;
461 
462  fault:
463 	return TYPE_FAULT;
464 }
465 
466 /*
467  * LDM/STM alignment handler.
468  *
469  * There are 4 variants of this instruction:
470  *
471  * B = rn pointer before instruction, A = rn pointer after instruction
472  *              ------ increasing address ----->
473  *	        |    | r0 | r1 | ... | rx |    |
474  * PU = 01             B                    A
475  * PU = 11        B                    A
476  * PU = 00        A                    B
477  * PU = 10             A                    B
478  */
479 static int
480 do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *regs)
481 {
482 	unsigned int rd, rn, correction, nr_regs, regbits;
483 	unsigned long eaddr, newaddr;
484 
485 	if (LDM_S_BIT(instr))
486 		goto bad;
487 
488 	correction = 4; /* processor implementation defined */
489 	regs->ARM_pc += correction;
490 
491 	ai_multi += 1;
492 
493 	/* count the number of registers in the mask to be transferred */
494 	nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
495 
496 	rn = RN_BITS(instr);
497 	newaddr = eaddr = regs->uregs[rn];
498 
499 	if (!LDST_U_BIT(instr))
500 		nr_regs = -nr_regs;
501 	newaddr += nr_regs;
502 	if (!LDST_U_BIT(instr))
503 		eaddr = newaddr;
504 
505 	if (LDST_P_EQ_U(instr))	/* U = P */
506 		eaddr += 4;
507 
508 	/*
509 	 * For alignment faults on the ARM922T/ARM920T the MMU  makes
510 	 * the FSR (and hence addr) equal to the updated base address
511 	 * of the multiple access rather than the restored value.
512 	 * Switch this message off if we've got a ARM92[02], otherwise
513 	 * [ls]dm alignment faults are noisy!
514 	 */
515 #if !(defined CONFIG_CPU_ARM922T)  && !(defined CONFIG_CPU_ARM920T)
516 	/*
517 	 * This is a "hint" - we already have eaddr worked out by the
518 	 * processor for us.
519 	 */
520 	if (addr != eaddr) {
521 		printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
522 			"addr = %08lx, eaddr = %08lx\n",
523 			 instruction_pointer(regs), instr, addr, eaddr);
524 		show_regs(regs);
525 	}
526 #endif
527 
528 	if (user_mode(regs)) {
529 		for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
530 		     regbits >>= 1, rd += 1)
531 			if (regbits & 1) {
532 				if (LDST_L_BIT(instr)) {
533 					unsigned int val;
534 					get32t_unaligned_check(val, eaddr);
535 					regs->uregs[rd] = val;
536 				} else
537 					put32t_unaligned_check(regs->uregs[rd], eaddr);
538 				eaddr += 4;
539 			}
540 	} else {
541 		for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
542 		     regbits >>= 1, rd += 1)
543 			if (regbits & 1) {
544 				if (LDST_L_BIT(instr)) {
545 					unsigned int val;
546 					get32_unaligned_check(val, eaddr);
547 					regs->uregs[rd] = val;
548 				} else
549 					put32_unaligned_check(regs->uregs[rd], eaddr);
550 				eaddr += 4;
551 			}
552 	}
553 
554 	if (LDST_W_BIT(instr))
555 		regs->uregs[rn] = newaddr;
556 	if (!LDST_L_BIT(instr) || !(REGMASK_BITS(instr) & (1 << 15)))
557 		regs->ARM_pc -= correction;
558 	return TYPE_DONE;
559 
560 fault:
561 	regs->ARM_pc -= correction;
562 	return TYPE_FAULT;
563 
564 bad:
565 	printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n");
566 	return TYPE_ERROR;
567 }
568 
569 /*
570  * Convert Thumb ld/st instruction forms to equivalent ARM instructions so
571  * we can reuse ARM userland alignment fault fixups for Thumb.
572  *
573  * This implementation was initially based on the algorithm found in
574  * gdb/sim/arm/thumbemu.c. It is basically just a code reduction of same
575  * to convert only Thumb ld/st instruction forms to equivalent ARM forms.
576  *
577  * NOTES:
578  * 1. Comments below refer to ARM ARM DDI0100E Thumb Instruction sections.
579  * 2. If for some reason we're passed an non-ld/st Thumb instruction to
580  *    decode, we return 0xdeadc0de. This should never happen under normal
581  *    circumstances but if it does, we've got other problems to deal with
582  *    elsewhere and we obviously can't fix those problems here.
583  */
584 
585 static unsigned long
586 thumb2arm(u16 tinstr)
587 {
588 	u32 L = (tinstr & (1<<11)) >> 11;
589 
590 	switch ((tinstr & 0xf800) >> 11) {
591 	/* 6.5.1 Format 1: */
592 	case 0x6000 >> 11:				/* 7.1.52 STR(1) */
593 	case 0x6800 >> 11:				/* 7.1.26 LDR(1) */
594 	case 0x7000 >> 11:				/* 7.1.55 STRB(1) */
595 	case 0x7800 >> 11:				/* 7.1.30 LDRB(1) */
596 		return 0xe5800000 |
597 			((tinstr & (1<<12)) << (22-12)) |	/* fixup */
598 			(L<<20) |				/* L==1? */
599 			((tinstr & (7<<0)) << (12-0)) |		/* Rd */
600 			((tinstr & (7<<3)) << (16-3)) |		/* Rn */
601 			((tinstr & (31<<6)) >>			/* immed_5 */
602 				(6 - ((tinstr & (1<<12)) ? 0 : 2)));
603 	case 0x8000 >> 11:				/* 7.1.57 STRH(1) */
604 	case 0x8800 >> 11:				/* 7.1.32 LDRH(1) */
605 		return 0xe1c000b0 |
606 			(L<<20) |				/* L==1? */
607 			((tinstr & (7<<0)) << (12-0)) |		/* Rd */
608 			((tinstr & (7<<3)) << (16-3)) |		/* Rn */
609 			((tinstr & (7<<6)) >> (6-1)) |	 /* immed_5[2:0] */
610 			((tinstr & (3<<9)) >> (9-8));	 /* immed_5[4:3] */
611 
612 	/* 6.5.1 Format 2: */
613 	case 0x5000 >> 11:
614 	case 0x5800 >> 11:
615 		{
616 			static const u32 subset[8] = {
617 				0xe7800000,		/* 7.1.53 STR(2) */
618 				0xe18000b0,		/* 7.1.58 STRH(2) */
619 				0xe7c00000,		/* 7.1.56 STRB(2) */
620 				0xe19000d0,		/* 7.1.34 LDRSB */
621 				0xe7900000,		/* 7.1.27 LDR(2) */
622 				0xe19000b0,		/* 7.1.33 LDRH(2) */
623 				0xe7d00000,		/* 7.1.31 LDRB(2) */
624 				0xe19000f0		/* 7.1.35 LDRSH */
625 			};
626 			return subset[(tinstr & (7<<9)) >> 9] |
627 			    ((tinstr & (7<<0)) << (12-0)) |	/* Rd */
628 			    ((tinstr & (7<<3)) << (16-3)) |	/* Rn */
629 			    ((tinstr & (7<<6)) >> (6-0));	/* Rm */
630 		}
631 
632 	/* 6.5.1 Format 3: */
633 	case 0x4800 >> 11:				/* 7.1.28 LDR(3) */
634 		/* NOTE: This case is not technically possible. We're
635 		 *	 loading 32-bit memory data via PC relative
636 		 *	 addressing mode. So we can and should eliminate
637 		 *	 this case. But I'll leave it here for now.
638 		 */
639 		return 0xe59f0000 |
640 		    ((tinstr & (7<<8)) << (12-8)) |		/* Rd */
641 		    ((tinstr & 255) << (2-0));			/* immed_8 */
642 
643 	/* 6.5.1 Format 4: */
644 	case 0x9000 >> 11:				/* 7.1.54 STR(3) */
645 	case 0x9800 >> 11:				/* 7.1.29 LDR(4) */
646 		return 0xe58d0000 |
647 			(L<<20) |				/* L==1? */
648 			((tinstr & (7<<8)) << (12-8)) |		/* Rd */
649 			((tinstr & 255) << 2);			/* immed_8 */
650 
651 	/* 6.6.1 Format 1: */
652 	case 0xc000 >> 11:				/* 7.1.51 STMIA */
653 	case 0xc800 >> 11:				/* 7.1.25 LDMIA */
654 		{
655 			u32 Rn = (tinstr & (7<<8)) >> 8;
656 			u32 W = ((L<<Rn) & (tinstr&255)) ? 0 : 1<<21;
657 
658 			return 0xe8800000 | W | (L<<20) | (Rn<<16) |
659 				(tinstr&255);
660 		}
661 
662 	/* 6.6.1 Format 2: */
663 	case 0xb000 >> 11:				/* 7.1.48 PUSH */
664 	case 0xb800 >> 11:				/* 7.1.47 POP */
665 		if ((tinstr & (3 << 9)) == 0x0400) {
666 			static const u32 subset[4] = {
667 				0xe92d0000,	/* STMDB sp!,{registers} */
668 				0xe92d4000,	/* STMDB sp!,{registers,lr} */
669 				0xe8bd0000,	/* LDMIA sp!,{registers} */
670 				0xe8bd8000	/* LDMIA sp!,{registers,pc} */
671 			};
672 			return subset[(L<<1) | ((tinstr & (1<<8)) >> 8)] |
673 			    (tinstr & 255);		/* register_list */
674 		}
675 		/* Else fall through for illegal instruction case */
676 
677 	default:
678 		return BAD_INSTR;
679 	}
680 }
681 
682 /*
683  * Convert Thumb-2 32 bit LDM, STM, LDRD, STRD to equivalent instruction
684  * handlable by ARM alignment handler, also find the corresponding handler,
685  * so that we can reuse ARM userland alignment fault fixups for Thumb.
686  *
687  * @pinstr: original Thumb-2 instruction; returns new handlable instruction
688  * @regs: register context.
689  * @poffset: return offset from faulted addr for later writeback
690  *
691  * NOTES:
692  * 1. Comments below refer to ARMv7 DDI0406A Thumb Instruction sections.
693  * 2. Register name Rt from ARMv7 is same as Rd from ARMv6 (Rd is Rt)
694  */
695 static void *
696 do_alignment_t32_to_handler(unsigned long *pinstr, struct pt_regs *regs,
697 			    union offset_union *poffset)
698 {
699 	unsigned long instr = *pinstr;
700 	u16 tinst1 = (instr >> 16) & 0xffff;
701 	u16 tinst2 = instr & 0xffff;
702 
703 	switch (tinst1 & 0xffe0) {
704 	/* A6.3.5 Load/Store multiple */
705 	case 0xe880:		/* STM/STMIA/STMEA,LDM/LDMIA, PUSH/POP T2 */
706 	case 0xe8a0:		/* ...above writeback version */
707 	case 0xe900:		/* STMDB/STMFD, LDMDB/LDMEA */
708 	case 0xe920:		/* ...above writeback version */
709 		/* no need offset decision since handler calculates it */
710 		return do_alignment_ldmstm;
711 
712 	case 0xf840:		/* POP/PUSH T3 (single register) */
713 		if (RN_BITS(instr) == 13 && (tinst2 & 0x09ff) == 0x0904) {
714 			u32 L = !!(LDST_L_BIT(instr));
715 			const u32 subset[2] = {
716 				0xe92d0000,	/* STMDB sp!,{registers} */
717 				0xe8bd0000,	/* LDMIA sp!,{registers} */
718 			};
719 			*pinstr = subset[L] | (1<<RD_BITS(instr));
720 			return do_alignment_ldmstm;
721 		}
722 		/* Else fall through for illegal instruction case */
723 		break;
724 
725 	/* A6.3.6 Load/store double, STRD/LDRD(immed, lit, reg) */
726 	case 0xe860:
727 	case 0xe960:
728 	case 0xe8e0:
729 	case 0xe9e0:
730 		poffset->un = (tinst2 & 0xff) << 2;
731 	case 0xe940:
732 	case 0xe9c0:
733 		return do_alignment_ldrdstrd;
734 
735 	/*
736 	 * No need to handle load/store instructions up to word size
737 	 * since ARMv6 and later CPUs can perform unaligned accesses.
738 	 */
739 	default:
740 		break;
741 	}
742 	return NULL;
743 }
744 
745 static int
746 do_alignment(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
747 {
748 	union offset_union uninitialized_var(offset);
749 	unsigned long instr = 0, instrptr;
750 	int (*handler)(unsigned long addr, unsigned long instr, struct pt_regs *regs);
751 	unsigned int type;
752 	unsigned int fault;
753 	u16 tinstr = 0;
754 	int isize = 4;
755 	int thumb2_32b = 0;
756 
757 	if (interrupts_enabled(regs))
758 		local_irq_enable();
759 
760 	instrptr = instruction_pointer(regs);
761 
762 	if (thumb_mode(regs)) {
763 		u16 *ptr = (u16 *)(instrptr & ~1);
764 		fault = probe_kernel_address(ptr, tinstr);
765 		if (!fault) {
766 			if (cpu_architecture() >= CPU_ARCH_ARMv7 &&
767 			    IS_T32(tinstr)) {
768 				/* Thumb-2 32-bit */
769 				u16 tinst2 = 0;
770 				fault = probe_kernel_address(ptr + 1, tinst2);
771 				instr = (tinstr << 16) | tinst2;
772 				thumb2_32b = 1;
773 			} else {
774 				isize = 2;
775 				instr = thumb2arm(tinstr);
776 			}
777 		}
778 	} else
779 		fault = probe_kernel_address(instrptr, instr);
780 
781 	if (fault) {
782 		type = TYPE_FAULT;
783 		goto bad_or_fault;
784 	}
785 
786 	if (user_mode(regs))
787 		goto user;
788 
789 	ai_sys += 1;
790 
791  fixup:
792 
793 	regs->ARM_pc += isize;
794 
795 	switch (CODING_BITS(instr)) {
796 	case 0x00000000:	/* 3.13.4 load/store instruction extensions */
797 		if (LDSTHD_I_BIT(instr))
798 			offset.un = (instr & 0xf00) >> 4 | (instr & 15);
799 		else
800 			offset.un = regs->uregs[RM_BITS(instr)];
801 
802 		if ((instr & 0x000000f0) == 0x000000b0 || /* LDRH, STRH */
803 		    (instr & 0x001000f0) == 0x001000f0)   /* LDRSH */
804 			handler = do_alignment_ldrhstrh;
805 		else if ((instr & 0x001000f0) == 0x000000d0 || /* LDRD */
806 			 (instr & 0x001000f0) == 0x000000f0)   /* STRD */
807 			handler = do_alignment_ldrdstrd;
808 		else if ((instr & 0x01f00ff0) == 0x01000090) /* SWP */
809 			goto swp;
810 		else
811 			goto bad;
812 		break;
813 
814 	case 0x04000000:	/* ldr or str immediate */
815 		offset.un = OFFSET_BITS(instr);
816 		handler = do_alignment_ldrstr;
817 		break;
818 
819 	case 0x06000000:	/* ldr or str register */
820 		offset.un = regs->uregs[RM_BITS(instr)];
821 
822 		if (IS_SHIFT(instr)) {
823 			unsigned int shiftval = SHIFT_BITS(instr);
824 
825 			switch(SHIFT_TYPE(instr)) {
826 			case SHIFT_LSL:
827 				offset.un <<= shiftval;
828 				break;
829 
830 			case SHIFT_LSR:
831 				offset.un >>= shiftval;
832 				break;
833 
834 			case SHIFT_ASR:
835 				offset.sn >>= shiftval;
836 				break;
837 
838 			case SHIFT_RORRRX:
839 				if (shiftval == 0) {
840 					offset.un >>= 1;
841 					if (regs->ARM_cpsr & PSR_C_BIT)
842 						offset.un |= 1 << 31;
843 				} else
844 					offset.un = offset.un >> shiftval |
845 							  offset.un << (32 - shiftval);
846 				break;
847 			}
848 		}
849 		handler = do_alignment_ldrstr;
850 		break;
851 
852 	case 0x08000000:	/* ldm or stm, or thumb-2 32bit instruction */
853 		if (thumb2_32b) {
854 			offset.un = 0;
855 			handler = do_alignment_t32_to_handler(&instr, regs, &offset);
856 		} else {
857 			offset.un = 0;
858 			handler = do_alignment_ldmstm;
859 		}
860 		break;
861 
862 	default:
863 		goto bad;
864 	}
865 
866 	if (!handler)
867 		goto bad;
868 	type = handler(addr, instr, regs);
869 
870 	if (type == TYPE_ERROR || type == TYPE_FAULT) {
871 		regs->ARM_pc -= isize;
872 		goto bad_or_fault;
873 	}
874 
875 	if (type == TYPE_LDST)
876 		do_alignment_finish_ldst(addr, instr, regs, offset);
877 
878 	return 0;
879 
880  bad_or_fault:
881 	if (type == TYPE_ERROR)
882 		goto bad;
883 	/*
884 	 * We got a fault - fix it up, or die.
885 	 */
886 	do_bad_area(addr, fsr, regs);
887 	return 0;
888 
889  swp:
890 	printk(KERN_ERR "Alignment trap: not handling swp instruction\n");
891 
892  bad:
893 	/*
894 	 * Oops, we didn't handle the instruction.
895 	 */
896 	printk(KERN_ERR "Alignment trap: not handling instruction "
897 		"%0*lx at [<%08lx>]\n",
898 		isize << 1,
899 		isize == 2 ? tinstr : instr, instrptr);
900 	ai_skipped += 1;
901 	return 1;
902 
903  user:
904 	ai_user += 1;
905 
906 	if (ai_usermode & UM_WARN)
907 		printk("Alignment trap: %s (%d) PC=0x%08lx Instr=0x%0*lx "
908 		       "Address=0x%08lx FSR 0x%03x\n", current->comm,
909 			task_pid_nr(current), instrptr,
910 			isize << 1,
911 			isize == 2 ? tinstr : instr,
912 		        addr, fsr);
913 
914 	if (ai_usermode & UM_FIXUP)
915 		goto fixup;
916 
917 	if (ai_usermode & UM_SIGNAL) {
918 		siginfo_t si;
919 
920 		si.si_signo = SIGBUS;
921 		si.si_errno = 0;
922 		si.si_code = BUS_ADRALN;
923 		si.si_addr = (void __user *)addr;
924 
925 		force_sig_info(si.si_signo, &si, current);
926 	} else {
927 		/*
928 		 * We're about to disable the alignment trap and return to
929 		 * user space.  But if an interrupt occurs before actually
930 		 * reaching user space, then the IRQ vector entry code will
931 		 * notice that we were still in kernel space and therefore
932 		 * the alignment trap won't be re-enabled in that case as it
933 		 * is presumed to be always on from kernel space.
934 		 * Let's prevent that race by disabling interrupts here (they
935 		 * are disabled on the way back to user space anyway in
936 		 * entry-common.S) and disable the alignment trap only if
937 		 * there is no work pending for this thread.
938 		 */
939 		raw_local_irq_disable();
940 		if (!(current_thread_info()->flags & _TIF_WORK_MASK))
941 			set_cr(cr_no_alignment);
942 	}
943 
944 	return 0;
945 }
946 
947 /*
948  * This needs to be done after sysctl_init, otherwise sys/ will be
949  * overwritten.  Actually, this shouldn't be in sys/ at all since
950  * it isn't a sysctl, and it doesn't contain sysctl information.
951  * We now locate it in /proc/cpu/alignment instead.
952  */
953 static int __init alignment_init(void)
954 {
955 #ifdef CONFIG_PROC_FS
956 	struct proc_dir_entry *res;
957 
958 	res = proc_create("cpu/alignment", S_IWUSR | S_IRUGO, NULL,
959 			  &alignment_proc_fops);
960 	if (!res)
961 		return -ENOMEM;
962 #endif
963 
964 #ifdef CONFIG_CPU_CP15
965 	if (cpu_is_v6_unaligned()) {
966 		cr_alignment &= ~CR_A;
967 		cr_no_alignment &= ~CR_A;
968 		set_cr(cr_alignment);
969 		ai_usermode = safe_usermode(ai_usermode, false);
970 	}
971 #endif
972 
973 	hook_fault_code(FAULT_CODE_ALIGNMENT, do_alignment, SIGBUS, BUS_ADRALN,
974 			"alignment exception");
975 
976 	/*
977 	 * ARMv6K and ARMv7 use fault status 3 (0b00011) as Access Flag section
978 	 * fault, not as alignment error.
979 	 *
980 	 * TODO: handle ARMv6K properly. Runtime check for 'K' extension is
981 	 * needed.
982 	 */
983 	if (cpu_architecture() <= CPU_ARCH_ARMv6) {
984 		hook_fault_code(3, do_alignment, SIGBUS, BUS_ADRALN,
985 				"alignment exception");
986 	}
987 
988 	return 0;
989 }
990 
991 fs_initcall(alignment_init);
992