1 files changed, 87 insertions, 5 deletions
diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S
index 87ab4e157997..a4eaf4f920c5 100644
--- a/arch/arm/kernel/entry-header.S
+++ b/arch/arm/kernel/entry-header.S
@@ -36,11 +36,6 @@
 #endif
        .endm
-        .macro  get_thread_info, rd
-        mov     \rd, sp, lsr #13
-        mov     \rd, \rd, lsl #13
-        .endm
        .macro  alignment_trap, rtemp
 #ifdef CONFIG_ALIGNMENT_TRAP
        ldr     \rtemp, .LCcralign
@@ -49,6 +44,93 @@
 #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
+#ifndef CONFIG_THUMB2_KERNEL
+        .macro  svc_exit, rpsr
+        msr     spsr_cxsf, \rpsr
+        ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
+        .endm
+        .macro  restore_user_regs, fast = 0, offset = 0
+        ldr     r1, [sp, #\offset + S_PSR]      @ get calling cpsr
+        ldr     lr, [sp, #\offset + S_PC]!      @ get pc
+        msr     spsr_cxsf, r1                   @ save in spsr_svc
+        .if     \fast
+        ldmdb   sp, {r1 - lr}^                  @ get calling r1 - lr
+        .else
+        ldmdb   sp, {r0 - lr}^                  @ get calling r0 - lr
+        .endif
+        add     sp, sp, #S_FRAME_SIZE - S_PC
+        movs    pc, lr                          @ return & move spsr_svc into cpsr
+        .endm
+        .macro  get_thread_info, rd
+        mov     \rd, sp, lsr #13
+        mov     \rd, \rd, lsl #13
+        .endm
+#else   /* CONFIG_THUMB2_KERNEL */
+        .macro  svc_exit, rpsr
+        ldr     r0, [sp, #S_SP]                 @ top of the stack
+        ldr     r1, [sp, #S_PC]                 @ return address
+        tst     r0, #4                          @ orig stack 8-byte aligned?
+        stmdb   r0, {r1, \rpsr}                 @ rfe context
+        ldmia   sp, {r0 - r12}
+        ldr     lr, [sp, #S_LR]
+        addeq   sp, sp, #S_FRAME_SIZE - 8       @ aligned
+        addne   sp, sp, #S_FRAME_SIZE - 4       @ not aligned
+        rfeia   sp!
+        .endm
+        .macro  restore_user_regs, fast = 0, offset = 0
+        mov     r2, sp
+        load_user_sp_lr r2, r3, \offset + S_SP  @ calling sp, lr
+        ldr     r1, [sp, #\offset + S_PSR]      @ get calling cpsr
+        ldr     lr, [sp, #\offset + S_PC]       @ get pc
+        add     sp, sp, #\offset + S_SP
+        msr     spsr_cxsf, r1                   @ save in spsr_svc
+        .if     \fast
+        ldmdb   sp, {r1 - r12}                  @ get calling r1 - r12
+        .else
+        ldmdb   sp, {r0 - r12}                  @ get calling r0 - r12
+        .endif
+        add     sp, sp, #S_FRAME_SIZE - S_SP
+        movs    pc, lr                          @ return & move spsr_svc into cpsr
+        .endm
+        .macro  get_thread_info, rd
+        mov     \rd, sp
+        lsr     \rd, \rd, #13
+        mov     \rd, \rd, lsl #13
+        .endm
+#endif  /* !CONFIG_THUMB2_KERNEL */
 /*
 * These are the registers used in the syscall handler, and allow us to

diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S index 87ab4e157997..a4eaf4f920c5 100644 --- a/arch/arm/kernel/entry-header.S +++ b/arch/arm/kernel/entry-header.S
@@ -36,11 +36,6 @@
36	#endif	36	#endif
37	.endm	37	.endm
38		38
39	.macro get_thread_info, rd
40	mov \rd, sp, lsr #13
41	mov \rd, \rd, lsl #13
42	.endm
43
44	.macro alignment_trap, rtemp	39	.macro alignment_trap, rtemp
45	#ifdef CONFIG_ALIGNMENT_TRAP	40	#ifdef CONFIG_ALIGNMENT_TRAP
46	ldr \rtemp, .LCcralign	41	ldr \rtemp, .LCcralign
@@ -49,6 +44,93 @@
49	#endif	44	#endif
50	.endm	45	.endm
51		46
		47	@
		48	@ Store/load the USER SP and LR registers by switching to the SYS
		49	@ mode. Useful in Thumb-2 mode where "stm/ldm rd, {sp, lr}^" is not
		50	@ available. Should only be called from SVC mode
		51	@
		52	.macro store_user_sp_lr, rd, rtemp, offset = 0
		53	mrs \rtemp, cpsr
		54	eor \rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
		55	msr cpsr_c, \rtemp @ switch to the SYS mode
		56
		57	str sp, [\rd, #\offset] @ save sp_usr
		58	str lr, [\rd, #\offset + 4] @ save lr_usr
		59
		60	eor \rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
		61	msr cpsr_c, \rtemp @ switch back to the SVC mode
		62	.endm
		63
		64	.macro load_user_sp_lr, rd, rtemp, offset = 0
		65	mrs \rtemp, cpsr
		66	eor \rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
		67	msr cpsr_c, \rtemp @ switch to the SYS mode
		68
		69	ldr sp, [\rd, #\offset] @ load sp_usr
		70	ldr lr, [\rd, #\offset + 4] @ load lr_usr
		71
		72	eor \rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
		73	msr cpsr_c, \rtemp @ switch back to the SVC mode
		74	.endm
		75
		76	#ifndef CONFIG_THUMB2_KERNEL
		77	.macro svc_exit, rpsr
		78	msr spsr_cxsf, \rpsr
		79	ldmia sp, {r0 - pc}^ @ load r0 - pc, cpsr
		80	.endm
		81
		82	.macro restore_user_regs, fast = 0, offset = 0
		83	ldr r1, [sp, #\offset + S_PSR] @ get calling cpsr
		84	ldr lr, [sp, #\offset + S_PC]! @ get pc
		85	msr spsr_cxsf, r1 @ save in spsr_svc
		86	.if \fast
		87	ldmdb sp, {r1 - lr}^ @ get calling r1 - lr
		88	.else
		89	ldmdb sp, {r0 - lr}^ @ get calling r0 - lr
		90	.endif
		91	add sp, sp, #S_FRAME_SIZE - S_PC
		92	movs pc, lr @ return & move spsr_svc into cpsr
		93	.endm
		94
		95	.macro get_thread_info, rd
		96	mov \rd, sp, lsr #13
		97	mov \rd, \rd, lsl #13
		98	.endm
		99	#else /* CONFIG_THUMB2_KERNEL */
		100	.macro svc_exit, rpsr
		101	ldr r0, [sp, #S_SP] @ top of the stack
		102	ldr r1, [sp, #S_PC] @ return address
		103	tst r0, #4 @ orig stack 8-byte aligned?
		104	stmdb r0, {r1, \rpsr} @ rfe context
		105	ldmia sp, {r0 - r12}
		106	ldr lr, [sp, #S_LR]
		107	addeq sp, sp, #S_FRAME_SIZE - 8 @ aligned
		108	addne sp, sp, #S_FRAME_SIZE - 4 @ not aligned
		109	rfeia sp!
		110	.endm
		111
		112	.macro restore_user_regs, fast = 0, offset = 0
		113	mov r2, sp
		114	load_user_sp_lr r2, r3, \offset + S_SP @ calling sp, lr
		115	ldr r1, [sp, #\offset + S_PSR] @ get calling cpsr
		116	ldr lr, [sp, #\offset + S_PC] @ get pc
		117	add sp, sp, #\offset + S_SP
		118	msr spsr_cxsf, r1 @ save in spsr_svc
		119	.if \fast
		120	ldmdb sp, {r1 - r12} @ get calling r1 - r12
		121	.else
		122	ldmdb sp, {r0 - r12} @ get calling r0 - r12
		123	.endif
		124	add sp, sp, #S_FRAME_SIZE - S_SP
		125	movs pc, lr @ return & move spsr_svc into cpsr
		126	.endm
		127
		128	.macro get_thread_info, rd
		129	mov \rd, sp
		130	lsr \rd, \rd, #13
		131	mov \rd, \rd, lsl #13
		132	.endm
		133	#endif /* !CONFIG_THUMB2_KERNEL */
52		134
53	/*	135	/*
54	* These are the registers used in the syscall handler, and allow us to	136	* These are the registers used in the syscall handler, and allow us to