From da3b36172d546200a851f165bf574d6090f4ff4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arve=20Hj=C3=B8nnev=C3=A5g?= Date: Mon, 24 Jun 2013 18:02:05 -0700 Subject: ARM: fiq_glue: Add custom fiq return handler api. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Change-Id: I5ff2764e85151ca0a88576542fda07c2d33dd065 Signed-off-by: Arve Hjønnevåg --- arch/arm/common/fiq_glue.S | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) (limited to 'arch/arm/common/fiq_glue.S') diff --git a/arch/arm/common/fiq_glue.S b/arch/arm/common/fiq_glue.S index 9e3455a09f8..24b42cec481 100644 --- a/arch/arm/common/fiq_glue.S +++ b/arch/arm/common/fiq_glue.S @@ -22,13 +22,14 @@ /* fiq stack: r0-r15,cpsr,spsr of interrupted mode */ ENTRY(fiq_glue) - /* store pc, cpsr from previous mode */ + /* store pc, cpsr from previous mode, reserve space for spsr */ mrs r12, spsr - sub r11, lr, #4 + sub lr, lr, #4 subs r10, #1 bne nested_fiq - stmfd sp!, {r11-r12, lr} + str r12, [sp, #-8]! + str lr, [sp, #-4]! /* store r8-r14 from previous mode */ sub sp, sp, #(7 * 4) @@ -85,12 +86,15 @@ fiq_from_usr_mode_exit: msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ldmfd sp!, {r0-r7} - add sp, sp, #(7 * 4) - ldmfd sp!, {r11-r12, lr} + ldr lr, [sp, #(4 * 7)] + ldr r12, [sp, #(4 * 8)] + add sp, sp, #(10 * 4) exit_fiq: msr spsr_cxsf, r12 add r10, #1 - movs pc, r11 + cmp r11, #0 + moveqs pc, lr + bx r11 /* jump to custom fiq return function */ nested_fiq: orr r12, r12, #(PSR_F_BIT) @@ -98,14 +102,17 @@ nested_fiq: fiq_glue_end: -ENTRY(fiq_glue_setup) /* func, data, sp */ - mrs r3, cpsr +ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */ + stmfd sp!, {r4} + mrs r4, cpsr msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) movs r8, r0 mov r9, r1 mov sp, r2 + mov r11, r3 moveq r10, #0 movne r10, #1 - msr cpsr_c, r3 + msr cpsr_c, r4 + ldmfd sp!, {r4} bx lr -- cgit v1.2.3