Thumb-2: Implementation of the unified start-up and exceptions code

This patch implements the ARM/Thumb-2 unified kernel start-up and
exception handling code.
Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>

arch/arm/kernel/debug.S

@@ -9,6 +9,8 @@
  *
  *  32-bit debugging code
  */
+#include <asm/unified.h>
+
 #include <linux/linkage.h>
 
 		.text
@@ -105,6 +107,7 @@ printhex:	adr	r2, hexbuf
 1:		and	r1, r0, #15
 		mov	r0, r0, lsr #4
 		cmp	r1, #10
+		ite	lt
 		addlt	r1, r1, #'0'
 		addge	r1, r1, #'a' - 10
 		strb	r1, [r3, #-1]!
@@ -123,9 +126,11 @@ ENTRY(printascii)
 		senduart r1, r3
 		busyuart r2, r3
 		teq	r1, #'\n'
+		itt	eq
 		moveq	r1, #'\r'
 		beq	1b
 2:		teq	r0, #0
+		itt	ne
 		ldrneb	r1, [r0], #1
 		teqne	r1, #0
 		bne	1b

arch/arm/kernel/entry-common.S

@@ -8,6 +8,8 @@
  * published by the Free Software Foundation.
  */
 
+#include <asm/unified.h>
+
 #include <asm/unistd.h>
 #include <asm/arch/entry-macro.S>
 
@@ -30,12 +32,18 @@ ret_fast_syscall:
 	arch_ret_to_user r1, lr
 
 	@ fast_restore_user_regs
+ THUMB(	mov	r2, sp			     )
+ THUMB(	load_user_sp_lr r2, r3, S_OFF + S_SP )	@ calling sp, lr
 	ldr	r1, [sp, #S_OFF + S_PSR]	@ get calling cpsr
-	ldr	lr, [sp, #S_OFF + S_PC]!	@ get pc
+ ARM(	ldr	lr, [sp, #S_OFF + S_PC]!     )	@ get pc
+ THUMB(	ldr	lr, [sp, #S_OFF + S_PC]	     )	@ get pc
+ THUMB(	add	sp, sp, #S_OFF + S_R1	     )
 	msr	spsr_cxsf, r1			@ save in spsr_svc
-	ldmdb	sp, {r1 - lr}^			@ get calling r1 - lr
+ ARM(	ldmdb	sp, {r1 - lr}^		     )	@ get calling r1 - lr
+ THUMB(	ldmia	sp, {r1 - r12}		     )	@ get calling r1 - r12
 	mov	r0, r0
-	add	sp, sp, #S_FRAME_SIZE - S_PC
+ ARM(	add	sp, sp, #S_FRAME_SIZE - S_PC )
+ THUMB(	add	sp, sp, #S_FRAME_SIZE - S_R1 )
 	movs	pc, lr				@ return & move spsr_svc into cpsr
 
 /*
@@ -69,12 +77,17 @@ no_work_pending:
 	arch_ret_to_user r1, lr
 
 	@ slow_restore_user_regs
+ THUMB(	mov	r2, sp			     )
+ THUMB(	load_user_sp_lr r2, r3, S_SP	     )	@ calling sp, lr
 	ldr	r1, [sp, #S_PSR]		@ get calling cpsr
-	ldr	lr, [sp, #S_PC]!		@ get pc
+ ARM(	ldr	lr, [sp, #S_PC]!	     )	@ get pc
+ THUMB(	ldr	lr, [sp, #S_PC]		     )	@ get pc
 	msr	spsr_cxsf, r1			@ save in spsr_svc
-	ldmdb	sp, {r0 - lr}^			@ get calling r1 - lr
+ ARM(	ldmdb	sp, {r0 - lr}^		     )	@ get calling r0 - lr
+ THUMB(	ldmia	sp, {r0 - r12}		     )	@ get calling r0 - r12
 	mov	r0, r0
-	add	sp, sp, #S_FRAME_SIZE - S_PC
+ ARM(	add	sp, sp, #S_FRAME_SIZE - S_PC )
+ THUMB(	add	sp, sp, #S_FRAME_SIZE	     )
 	movs	pc, lr				@ return & move spsr_svc into cpsr
 ENDPROC(ret_to_user)
 
@@ -123,8 +136,10 @@ ENDPROC(ret_from_fork)
 ENTRY(vector_swi)
 	sub	sp, sp, #S_FRAME_SIZE
 	stmia	sp, {r0 - r12}			@ Calling r0 - r12
-	add	r8, sp, #S_PC
-	stmdb	r8, {sp, lr}^			@ Calling sp, lr
+ ARM(	add	r8, sp, #S_PC		)
+ ARM(	stmdb	r8, {sp, lr}^		)	@ Calling sp, lr
+ THUMB(	mov	r8, sp			)
+ THUMB(	store_user_sp_lr r8, r10, S_SP	)	@ calling sp, lr
 	mrs	r8, spsr			@ called from non-FIQ mode, so ok.
 	str	lr, [sp, #S_PC]			@ Save calling PC
 	str	r8, [sp, #S_PSR]		@ Save CPSR
@@ -143,6 +158,7 @@ ENTRY(vector_swi)
 	 */
 #ifdef CONFIG_ARM_THUMB
 	tst	r8, #PSR_T_BIT
+	ite	ne
 	movne	r10, #0				@ no thumb OABI emulation
 	ldreq	r10, [lr, #-4]			@ get SWI instruction
 #else
@@ -198,6 +214,7 @@ ENTRY(vector_swi)
 	 * get the old ABI syscall table address.
 	 */
 	bics	r10, r10, #0xff000000
+	itt	ne
 	eorne	scno, r10, #__NR_OABI_SYSCALL_BASE
 	ldrne	tbl, =sys_oabi_call_table
 #elif !defined(CONFIG_AEABI)
@@ -210,7 +227,8 @@ ENTRY(vector_swi)
 	bne	__sys_trace
 
 	cmp	scno, #NR_syscalls		@ check upper syscall limit
-	adr	lr, ret_fast_syscall		@ return address
+	badr	lr, ret_fast_syscall		@ return address
+	it	cc
 	ldrcc	pc, [tbl, scno, lsl #2]		@ call sys_* routine
 
 	add	r1, sp, #S_OFF
@@ -231,10 +249,11 @@ __sys_trace:
 	mov	r0, #0				@ trace entry [IP = 0]
 	bl	syscall_trace
 
-	adr	lr, __sys_trace_return		@ return address
+	badr	lr, __sys_trace_return		@ return address
 	mov	scno, r0			@ syscall number (possibly new)
 	add	r1, sp, #S_R0 + S_OFF		@ pointer to regs
 	cmp	scno, #NR_syscalls		@ check upper syscall limit
+	itt	cc
 	ldmccia	r1, {r0 - r3}			@ have to reload r0 - r3
 	ldrcc	pc, [tbl, scno, lsl #2]		@ call sys_* routine
 	b	2b
@@ -280,11 +299,14 @@ ENTRY(sys_call_table)
 sys_syscall:
 		bic	scno, r0, #__NR_OABI_SYSCALL_BASE
 		cmp	scno, #__NR_syscall - __NR_SYSCALL_BASE
+		it	ne
 		cmpne	scno, #NR_syscalls	@ check range
+		itttt	lo
 		stmloia	sp, {r5, r6}		@ shuffle args
 		movlo	r0, r1
 		movlo	r1, r2
 		movlo	r2, r3
+		itt	lo
 		movlo	r3, r4
 		ldrlo	pc, [tbl, scno, lsl #2]
 		b	sys_ni_syscall
@@ -338,12 +360,14 @@ ENDPROC(sys_sigaltstack_wrapper)
 
 sys_statfs64_wrapper:
 		teq	r1, #88
+		it	eq
 		moveq	r1, #84
 		b	sys_statfs64
 ENDPROC(sys_statfs64_wrapper)
 
 sys_fstatfs64_wrapper:
 		teq	r1, #88
+		it	eq
 		moveq	r1, #84
 		b	sys_fstatfs64
 ENDPROC(sys_fstatfs64_wrapper)
@@ -355,6 +379,7 @@ ENDPROC(sys_fstatfs64_wrapper)
 sys_mmap2:
 #if PAGE_SHIFT > 12
 		tst	r5, #PGOFF_MASK
+		ittt	eq
 		moveq	r5, r5, lsr #PAGE_SHIFT - 12
 		streq	r5, [sp, #4]
 		beq	do_mmap2

arch/arm/kernel/entry-header.S

@@ -37,7 +37,9 @@
 	.endm
 
 	.macro	get_thread_info, rd
-	mov	\rd, sp, lsr #13
+ ARM(	mov	\rd, sp, lsr #13	)
+ THUMB(	mov	\rd, sp			)
+ THUMB(	lsr	\rd, \rd, #13		)
 	mov	\rd, \rd, lsl #13
 	.endm
 
@@ -49,6 +51,54 @@
 #endif
 	.endm
 
+	@
+	@ Store/load the USER SP and LR registers by switching to the SYS
+	@ mode. Useful in Thumb-2 mode where "stm/ldm rd, {sp, lr}^" is not
+	@ available. Should only be called from SVC mode
+	@
+	.macro	store_user_sp_lr, rd, rtemp, offset = 0
+	mrs	\rtemp, cpsr
+	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
+	msr	cpsr_c, \rtemp			@ switch to the SYS mode
+
+	str	sp, [\rd, #\offset]		@ save sp_usr
+	str	lr, [\rd, #\offset + 4]		@ save lr_usr
+
+	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
+	msr	cpsr_c, \rtemp			@ switch back to the SVC mode
+	.endm
+
+	.macro	load_user_sp_lr, rd, rtemp, offset = 0
+	mrs	\rtemp, cpsr
+	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
+	msr	cpsr_c, \rtemp			@ switch to the SYS mode
+
+	ldr	sp, [\rd, #\offset]		@ load sp_usr
+	ldr	lr, [\rd, #\offset + 4]		@ load lr_usr
+
+	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
+	msr	cpsr_c, \rtemp			@ switch back to the SVC mode
+	.endm
+
+	.macro	arm_rfe, rpsr
+	ldr	\rpsr, [sp, #S_PSR]
+	msr	spsr_cxsf, \rpsr
+	ldmia	sp, {r0 - pc}^			@ load r0 - pc, cpsr
+	.endm
+
+	.macro	thumb_rfe, rsp, rpc, rpsr
+	ldr	\rsp, [sp, #S_SP]		@ top of the stack
+	ldr	\rpc, [sp, #S_PC]		@ return address
+	ldr	\rpsr, [sp, #S_PSR]		@ return cpsr
+	tst	\rsp, #4			@ orig stack 8-byte aligned?
+	stmdb	\rsp, {\rpc, \rpsr}		@ rfe context
+	ldmia	sp, {r0 - r12}
+	ldr	lr, [sp, #S_LR]
+	ite	eq
+	addeq	sp, sp, #S_FRAME_SIZE - 8	@ aligned
+	addne	sp, sp, #S_FRAME_SIZE - 4	@ not aligned
+	rfeia	sp!
+	.endm
 
 /*
  * These are the registers used in the syscall handler, and allow us to

arch/arm/kernel/head-common.S

@@ -10,6 +10,7 @@
  * published by the Free Software Foundation.
  *
  */
+#include <asm/unified.h>
 
 	.type	__switch_data, %object
 __switch_data:
@@ -38,6 +39,7 @@ __mmap_switched:
 
 	ldmia	r3!, {r4, r5, r6, r7}
 	cmp	r4, r5				@ Copy data segment if needed
+	itttt	ne
 1:	cmpne	r5, r6
 	ldrne	fp, [r4], #4
 	strne	fp, [r5], #4
@@ -45,10 +47,13 @@ __mmap_switched:
 
 	mov	fp, #0				@ Clear BSS (and zero fp)
 1:	cmp	r6, r7
+	itt	cc
 	strcc	fp, [r6],#4
 	bcc	1b
 
-	ldmia	r3, {r4, r5, r6, r7, sp}
+ ARM(	ldmia	r3, {r4, r5, r6, r7, sp})
+ THUMB(	ldmia	r3, {r4, r5, r6, r7}	)
+ THUMB(	ldr	sp, [r3, #16]		)
 	str	r9, [r4]			@ Save processor ID
 	str	r1, [r5]			@ Save machine type
 	str	r2, [r6]			@ Save atags pointer
@@ -146,8 +151,11 @@ ENDPROC(__error)
  *	r9 = cpuid (preserved)
  */
 __lookup_processor_type:
-	adr	r3, 3f
-	ldmda	r3, {r5 - r7}
+ ARM(	adr	r3, 3f		)
+ ARM(	ldmda	r3, {r5 - r7}	)
+ THUMB(	adr	r3, 3f+4	)
+ THUMB(	ldmdb	r3, {r5 - r7}	)
+ THUMB(	sub	r3, r3, #4	)
 	sub	r3, r3, r7			@ get offset between virt&phys
 	add	r5, r5, r3			@ convert virt addresses to
 	add	r6, r6, r3			@ physical address space

arch/arm/kernel/head-nommu.S

@@ -11,6 +11,8 @@
  *  Common kernel startup code (non-paged MM)
  *
  */
+#include <asm/unified.h>
+
 #include <linux/linkage.h>
 #include <linux/init.h>
 
@@ -38,8 +40,9 @@
  */
 	.section ".text.head", "ax"
 ENTRY(stext)
-	msr	cpsr_c, #PSR_F_BIT | PSR_I_BIT | SVC_MODE @ ensure svc mode
-						@ and irqs disabled
+ ARM(	msr	cpsr_c, #PSR_F_BIT | PSR_I_BIT | SVC_MODE ) @ ensure svc mode
+ THUMB(	mov	r9, #PSR_F_BIT | PSR_I_BIT | SVC_MODE )
+ THUMB(	msr	cpsr_c, r9 )			@ ensure svc mode
 #ifndef CONFIG_CPU_CP15
 	ldr	r9, =CONFIG_PROCESSOR_ID
 #else
@@ -54,8 +57,10 @@ ENTRY(stext)
 
 	ldr	r13, __switch_data		@ address to jump to after
 						@ the initialization is done
-	adr	lr, __after_proc_init		@ return (PIC) address
-	add	pc, r10, #PROCINFO_INITFUNC
+	badr	lr, __after_proc_init		@ return (PIC) address
+ ARM(	add	pc, r10, #PROCINFO_INITFUNC	)
+ THUMB(	add	r12, r10, #PROCINFO_INITFUNC	)
+ THUMB(	mov	pc, r12				)
 ENDPROC(stext)
 
 /*

arch/arm/kernel/head.S

@@ -11,6 +11,8 @@
  *
  *  Kernel startup code for all 32-bit CPUs
  */
+#include <asm/unified.h>
+
 #include <linux/linkage.h>
 #include <linux/init.h>
 
@@ -79,7 +81,9 @@
  */
 	.section ".text.head", "ax"
 ENTRY(stext)
-	msr	cpsr_c, #PSR_F_BIT | PSR_I_BIT | SVC_MODE @ ensure svc mode
+ ARM(	msr	cpsr_c, #PSR_F_BIT | PSR_I_BIT | SVC_MODE ) @ ensure svc mode
+ THUMB(	mov	r9, #PSR_F_BIT | PSR_I_BIT | SVC_MODE )
+ THUMB(	msr	cpsr_c, r9 )			@ ensure svc mode
 						@ and irqs disabled
 	mrc	p15, 0, r9, c0, c0		@ get processor id
 	bl	__lookup_processor_type		@ r5=procinfo r9=cpuid
@@ -100,8 +104,10 @@ ENTRY(stext)
 	 */
 	ldr	r13, __switch_data		@ address to jump to after
 						@ mmu has been enabled
-	adr	lr, __enable_mmu		@ return (PIC) address
-	add	pc, r10, #PROCINFO_INITFUNC
+	badr	lr, __enable_mmu		@ return (PIC) address
+ ARM(	add	pc, r10, #PROCINFO_INITFUNC	)
+ THUMB(	add	r12, r10, #PROCINFO_INITFUNC	)
+ THUMB(	mov	pc, r12				)
 ENDPROC(stext)
 
 #if defined(CONFIG_SMP)
@@ -127,9 +133,11 @@ ENTRY(secondary_startup)
 	ldmia	r4, {r5, r7, r13}		@ address to jump to after
 	sub	r4, r4, r5			@ mmu has been enabled
 	ldr	r4, [r7, r4]			@ get secondary_data.pgdir
-	adr	lr, __enable_mmu		@ return address
-	add	pc, r10, #PROCINFO_INITFUNC	@ initialise processor
-						@ (return control reg)
+	badr	lr, __enable_mmu		@ return address
+ ARM(	add	pc, r10, #PROCINFO_INITFUNC	) @ initialise processor
+						  @ (return control reg)
+ THUMB(	add	r12, r10, #PROCINFO_INITFUNC	)
+ THUMB(	mov	pc, r12				)
 ENDPROC(secondary_startup)
 
 	/*
@@ -253,6 +261,7 @@ __create_page_tables:
 	add	r6, r4, r6, lsr #18
 1:	cmp	r0, r6
 	add	r3, r3, #1 << 20
+	it	ls
 	strls	r3, [r0], #4
 	bls	1b
 
@@ -296,6 +305,7 @@ __create_page_tables:
 	add	r0, r4, r3
 	rsb	r3, r3, #0x4000			@ PTRS_PER_PGD*sizeof(long)
 	cmp	r3, #0x0800			@ limit to 512MB
+	it	hi
 	movhi	r3, #0x0800
 	add	r6, r0, r3
 	ldr	r3, [r8, #MACHINFO_PHYSIO]

arch/arm/kernel/process.c

@@ -30,6 +30,7 @@
 #include <linux/tick.h>
 #include <linux/utsname.h>
 
+#include <asm/unified.h>
 #include <asm/leds.h>
 #include <asm/processor.h>
 #include <asm/system.h>
@@ -425,7 +426,7 @@ pid_t kernel_thread(int (*fn)(void *), void *arg, unsigned long flags)
 	regs.ARM_r2 = (unsigned long)fn;
 	regs.ARM_r3 = (unsigned long)do_exit;
 	regs.ARM_pc = (unsigned long)kernel_thread_helper;
-	regs.ARM_cpsr = SVC_MODE;
+	regs.ARM_cpsr = SVC_MODE | PSR_ISETSTATE;
 
 	return do_fork(flags|CLONE_VM|CLONE_UNTRACED, 0, &regs, 0, NULL, NULL);
 }

arch/arm/kernel/setup.c

@@ -26,6 +26,7 @@
 #include <linux/fs.h>
 #include <linux/kexec.h>
 
+#include <asm/unified.h>
 #include <asm/cpu.h>
 #include <asm/elf.h>
 #include <asm/procinfo.h>
@@ -410,13 +411,17 @@ void cpu_init(void)
 	"msr	cpsr_c, %7"
 	    :
 	    : "r" (stk),
-	      "I" (PSR_F_BIT | PSR_I_BIT | IRQ_MODE),
+	ARM(  "I" (PSR_F_BIT | PSR_I_BIT | IRQ_MODE),	)
+	THUMB("r" (PSR_F_BIT | PSR_I_BIT | IRQ_MODE),	)
 	      "I" (offsetof(struct stack, irq[0])),
-	      "I" (PSR_F_BIT | PSR_I_BIT | ABT_MODE),
+	ARM(  "I" (PSR_F_BIT | PSR_I_BIT | ABT_MODE),	)
+	THUMB("r" (PSR_F_BIT | PSR_I_BIT | ABT_MODE),	)
 	      "I" (offsetof(struct stack, abt[0])),
-	      "I" (PSR_F_BIT | PSR_I_BIT | UND_MODE),
+	ARM(  "I" (PSR_F_BIT | PSR_I_BIT | UND_MODE),	)
+	THUMB("r" (PSR_F_BIT | PSR_I_BIT | UND_MODE),	)
 	      "I" (offsetof(struct stack, und[0])),
-	      "I" (PSR_F_BIT | PSR_I_BIT | SVC_MODE)
+	ARM(  "I" (PSR_F_BIT | PSR_I_BIT | SVC_MODE)	)
+	THUMB("r" (PSR_F_BIT | PSR_I_BIT | SVC_MODE)	)
 	    : "r14");
 }