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
@@ -86,14 +88,17 @@
 /*
  * Useful debugging routines
  */
+		.type	printhex8, %function
 ENTRY(printhex8)
 		mov	r1, #8
 		b	printhex
 
+		.type	printhex4, %function
 ENTRY(printhex4)
 		mov	r1, #4
 		b	printhex
 
+		.type	printhex2, %function
 ENTRY(printhex2)
 		mov	r1, #2
 printhex:	adr	r2, hexbuf
@@ -103,6 +108,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]!
@@ -113,6 +119,7 @@ printhex:	adr	r2, hexbuf
 
 		.ltorg
 
+		.type	printascii, %function
 ENTRY(printascii)
 		addruart r3
 		b	2f
@@ -120,14 +127,17 @@ 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
 		mov	pc, lr
 
+		.type	printch, %function
 ENTRY(printch)
 		addruart r3
 		mov	r1, r0

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
 
 /*
@@ -58,6 +66,7 @@ work_resched:
 /*
  * "slow" syscall return path.  "why" tells us if this was a real syscall.
  */
+	.type	ret_to_user, %function
 ENTRY(ret_to_user)
 ret_slow_syscall:
 	disable_irq				@ disable interrupts
@@ -69,17 +78,23 @@ 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 r1 - 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
 
 /*
  * This is how we return from a fork.
  */
+	.type	ret_from_fork, %function
 ENTRY(ret_from_fork)
 	bl	schedule_tail
 	get_thread_info tsk
@@ -119,11 +134,14 @@ ENTRY(ret_from_fork)
 #endif
 
 	.align	5
+	.type	vector_swi, %function
 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
@@ -142,6 +160,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
@@ -197,6 +216,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)
@@ -209,7 +229,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
@@ -229,10 +250,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
@@ -279,59 +301,75 @@ 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
 
+                .type   sys_fork_wrapper, %function
 sys_fork_wrapper:
 		add	r0, sp, #S_OFF
 		b	sys_fork
 
+                .type   sys_vfork_wrapper, %function
 sys_vfork_wrapper:
 		add	r0, sp, #S_OFF
 		b	sys_vfork
 
+                .type   sys_execve_wrapper, %function
 sys_execve_wrapper:
 		add	r3, sp, #S_OFF
 		b	sys_execve
 
+                .type   sys_clone_wrapper, %function
 sys_clone_wrapper:
 		add	ip, sp, #S_OFF
 		str	ip, [sp, #4]
 		b	sys_clone
 
+                .type   sys_sigsuspend_wrapper, %function
 sys_sigsuspend_wrapper:
 		add	r3, sp, #S_OFF
 		b	sys_sigsuspend
 
+                .type   sys_rt_sigsuspend_wrapper, %function
 sys_rt_sigsuspend_wrapper:
 		add	r2, sp, #S_OFF
 		b	sys_rt_sigsuspend
 
+                .type   sys_sigreturn_wrapper, %function
 sys_sigreturn_wrapper:
 		add	r0, sp, #S_OFF
 		b	sys_sigreturn
 
+                .type   sys_rt_sigreturn_wrapper, %function
 sys_rt_sigreturn_wrapper:
 		add	r0, sp, #S_OFF
 		b	sys_rt_sigreturn
 
+                .type   sys_sigaltstack_wrapper, %function
 sys_sigaltstack_wrapper:
 		ldr	r2, [sp, #S_OFF + S_SP]
 		b	do_sigaltstack
 
+                .type   sys_statfs64_wrapper, %function
 sys_statfs64_wrapper:
 		teq	r1, #88
+		it	eq
 		moveq	r1, #84
 		b	sys_statfs64
 
+                .type   sys_fstatfs64_wrapper, %function
 sys_fstatfs64_wrapper:
 		teq	r1, #88
+		it	eq
 		moveq	r1, #84
 		b	sys_fstatfs64
 
@@ -339,9 +377,11 @@ sys_fstatfs64_wrapper:
  * Note: off_4k (r5) is always units of 4K.  If we can't do the requested
  * offset, we return EINVAL.
  */
+                .type   sys_mmap2, %function
 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
@@ -352,8 +392,10 @@ sys_mmap2:
 		b	do_mmap2
 #endif
 
+	.type	pabort_ifar, %function
 ENTRY(pabort_ifar)
 		mrc	p15, 0, r0, cr6, cr0, 2
+	.type	pabort_noifar, %function
 ENTRY(pabort_noifar)
 		mov	pc, lr
 
@@ -363,24 +405,29 @@ ENTRY(pabort_noifar)
  * These are syscalls with argument register differences
  */
 
+                .type   sys_oabi_pread64, %function
 sys_oabi_pread64:
 		stmia	sp, {r3, r4}
 		b	sys_pread64
 
+                .type   sys_oabi_pwrite64, %function
 sys_oabi_pwrite64:
 		stmia	sp, {r3, r4}
 		b	sys_pwrite64
 
+                .type   sys_oabi_truncate64, %function
 sys_oabi_truncate64:
 		mov	r3, r2
 		mov	r2, r1
 		b	sys_truncate64
 
+                .type   sys_oabi_ftruncate64, %function
 sys_oabi_ftruncate64:
 		mov	r3, r2
 		mov	r2, r1
 		b	sys_ftruncate64
 
+                .type   sys_oabi_readahead, %function
 sys_oabi_readahead:
 		str	r3, [sp]
 		mov	r3, r2

arch/arm/kernel/entry-header.S

@@ -49,6 +49,54 @@
 #endif
 	.endm
 
+	@
+	@ Store/load the USER SP and LR registers by switching to the SYS
+	@ mode. Useful in Thumb-2 more 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:
@@ -37,6 +38,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
@@ -44,10 +46,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, sp}
+ ARM(	ldmia	r3, {r4, r5, r6, sp}	)
+ THUMB(	ldmia	r3, {r4, r5, r6}	)
+ THUMB(	ldr	sp, [r3, #12]		)
 	str	r9, [r4]			@ Save processor ID
 	str	r1, [r5]			@ Save machine type
 	bic	r4, r0, #CR_A			@ Clear 'A' bit
@@ -144,8 +149,11 @@ __error:
  */
 	.type	__lookup_processor_type, %function
 __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
@@ -162,6 +170,7 @@ __lookup_processor_type:
 /*
  * This provides a C-API version of the above function.
  */
+	.type	lookup_processor_type, %function
 ENTRY(lookup_processor_type)
 	stmfd	sp!, {r4 - r7, r9, lr}
 	mov	r9, r0
@@ -209,6 +218,7 @@ __lookup_machine_type:
 /*
  * This provides a C-API version of the above function.
  */
+	.type	lookup_machine_type, %function
 ENTRY(lookup_machine_type)
 	stmfd	sp!, {r4 - r6, lr}
 	mov	r1, r0

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>
 
@@ -52,7 +54,7 @@ ENTRY(stext)
 
 	ldr	r13, __switch_data		@ address to jump to after
 						@ the initialization is done
-	adr	lr, __after_proc_init		@ return (PIC) address
+	badr	lr, __after_proc_init		@ return (PIC) address
 	add	pc, r10, #PROCINFO_INITFUNC
 
 /*

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>
 
@@ -76,7 +78,9 @@
 	__INIT
 	.type	stext, %function
 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
@@ -96,8 +100,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				)
 
 #if defined(CONFIG_SMP)
 	.type   secondary_startup, #function
@@ -123,13 +129,16 @@ 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
+	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				)
 
 	/*
 	 * r6  = &secondary_data
 	 */
+	.type	__secondary_switched, %function
 ENTRY(__secondary_switched)
 	ldr	sp, [r7, #4]			@ get secondary_data.stack
 	mov	fp, #0
@@ -249,6 +258,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
 
@@ -288,6 +298,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

@@ -29,6 +29,7 @@
 #include <linux/elfcore.h>
 #include <linux/pm.h>
 
+#include <asm/unified.h>
 #include <asm/leds.h>
 #include <asm/processor.h>
 #include <asm/system.h>
@@ -412,7 +413,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

@@ -24,6 +24,7 @@
 #include <linux/interrupt.h>
 #include <linux/smp.h>
 
+#include <asm/unified.h>
 #include <asm/cpu.h>
 #include <asm/elf.h>
 #include <asm/procinfo.h>
@@ -406,13 +407,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");
 }