From mboxrd@z Thu Jan 1 00:00:00 1970 Return-Path: Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1759848AbaIOXAW (ORCPT ); Mon, 15 Sep 2014 19:00:22 -0400 Received: from mail-pd0-f174.google.com ([209.85.192.174]:50147 "EHLO mail-pd0-f174.google.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1756154AbaIOXAQ (ORCPT ); Mon, 15 Sep 2014 19:00:16 -0400 Message-ID: <54176F7C.6030908@linaro.org> Date: Tue, 16 Sep 2014 00:00:12 +0100 From: Daniel Thompson User-Agent: Mozilla/5.0 (X11; Linux x86_64; rv:31.0) Gecko/20100101 Thunderbird/31.1.0 MIME-Version: 1.0 To: Russell King CC: linux-kernel@vger.kernel.org, linux-arm-kernel@lists.infradead.org, patches@linaro.org, linaro-kernel@lists.linaro.org, John Stultz , Thomas Gleixner , Sumit Semwal , Catalin Marinas Subject: Re: [PATCH 3.17-rc4 v6 2/6] arm: fiq: Replace default FIQ handler References: <1410695835-10496-1-git-send-email-daniel.thompson@linaro.org> <1410695835-10496-3-git-send-email-daniel.thompson@linaro.org> In-Reply-To: <1410695835-10496-3-git-send-email-daniel.thompson@linaro.org> Content-Type: text/plain; charset=windows-1252 Content-Transfer-Encoding: 8bit Sender: linux-kernel-owner@vger.kernel.org List-ID: X-Mailing-List: linux-kernel@vger.kernel.org On 14/09/14 12:57, Daniel Thompson wrote: > This patch introduces a new default FIQ handler that is structured in a > similar way to the existing ARM exception handler and result in the FIQ > being handled by C code running on the SVC stack (despite this code run > in the FIQ handler is subject to severe limitations with respect to > locking making normal interaction with the kernel impossible). > > This default handler allows concepts that on x86 would be handled using > NMIs to be realized on ARM. > > Credit: > > This patch is a near complete re-write of a patch originally > provided by Anton Vorontsov. Today only a couple of small fragments > survive, however without Anton's work to build from this patch would > not exist. Thanks also to Russell King for spoonfeeding me a variety > of fixes during the review cycle. I've send this patch to the your patch tracker as complete respin (#8150/2). If you'd rather handle it as a follow on patch please let me know and I will prepare it as one. Daniel. > > Signed-off-by: Daniel Thompson > Cc: Russell King > Cc: Catalin Marinas > Acked-by: Nicolas Pitre > --- > arch/arm/kernel/entry-armv.S | 98 +++++++++++++++++++++++++++++++++++++----- > arch/arm/kernel/entry-header.S | 47 ++++++++++++++++++++ > arch/arm/kernel/fiq.c | 11 ++++- > arch/arm/kernel/setup.c | 8 +++- > arch/arm/kernel/traps.c | 26 +++++++++++ > 5 files changed, 177 insertions(+), 13 deletions(-) > > diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S > index 36276cd..859f56c 100644 > --- a/arch/arm/kernel/entry-armv.S > +++ b/arch/arm/kernel/entry-armv.S > @@ -146,7 +146,7 @@ ENDPROC(__und_invalid) > #define SPFIX(code...) > #endif > > - .macro svc_entry, stack_hole=0 > + .macro svc_entry, stack_hole=0, trace=1 > UNWIND(.fnstart ) > UNWIND(.save {r0 - pc} ) > sub sp, sp, #(S_FRAME_SIZE + \stack_hole - 4) > @@ -182,9 +182,11 @@ ENDPROC(__und_invalid) > @ > stmia r7, {r2 - r6} > > + .if \trace > #ifdef CONFIG_TRACE_IRQFLAGS > bl trace_hardirqs_off > #endif > + .endif > .endm > > .align 5 > @@ -295,6 +297,15 @@ __pabt_svc: > ENDPROC(__pabt_svc) > > .align 5 > +__fiq_svc: > + svc_entry trace=0 > + mov r0, sp @ struct pt_regs *regs > + bl handle_fiq_as_nmi > + svc_exit_via_fiq > + UNWIND(.fnend ) > +ENDPROC(__fiq_svc) > + > + .align 5 > .LCcralign: > .word cr_alignment > #ifdef MULTI_DABORT > @@ -305,6 +316,46 @@ ENDPROC(__pabt_svc) > .word fp_enter > > /* > + * Abort mode handlers > + */ > + > +@ > +@ Taking a FIQ in abort mode is similar to taking a FIQ in SVC mode > +@ and reuses the same macros. However in abort mode we must also > +@ save/restore lr_abt and spsr_abt to make nested aborts safe. > +@ > + .align 5 > +__fiq_abt: > + svc_entry trace=0 > + > + ARM( msr cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + mov r1, lr @ Save lr_abt > + mrs r2, spsr @ Save spsr_abt, abort is now safe > + ARM( msr cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + stmfd sp!, {r1 - r2} > + > + add r0, sp, #8 @ struct pt_regs *regs > + bl handle_fiq_as_nmi > + > + ldmfd sp!, {r1 - r2} > + ARM( msr cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + mov lr, r1 @ Restore lr_abt, abort is unsafe > + msr spsr_cxsf, r2 @ Restore spsr_abt > + ARM( msr cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + > + svc_exit_via_fiq > + UNWIND(.fnend ) > +ENDPROC(__fiq_abt) > + > +/* > * User mode handlers > * > * EABI note: sp_svc is always 64-bit aligned here, so should S_FRAME_SIZE > @@ -314,7 +365,7 @@ ENDPROC(__pabt_svc) > #error "sizeof(struct pt_regs) must be a multiple of 8" > #endif > > - .macro usr_entry > + .macro usr_entry, trace=1 > UNWIND(.fnstart ) > UNWIND(.cantunwind ) @ don't unwind the user space > sub sp, sp, #S_FRAME_SIZE > @@ -351,10 +402,12 @@ ENDPROC(__pabt_svc) > @ > zero_fp > > + .if \trace > #ifdef CONFIG_IRQSOFF_TRACER > bl trace_hardirqs_off > #endif > ct_user_exit save = 0 > + .endif > .endm > > .macro kuser_cmpxchg_check > @@ -683,6 +736,17 @@ ENTRY(ret_from_exception) > ENDPROC(__pabt_usr) > ENDPROC(ret_from_exception) > > + .align 5 > +__fiq_usr: > + usr_entry trace=0 > + kuser_cmpxchg_check > + mov r0, sp @ struct pt_regs *regs > + bl handle_fiq_as_nmi > + get_thread_info tsk > + restore_user_regs fast = 0, offset = 0 > + UNWIND(.fnend ) > +ENDPROC(__fiq_usr) > + > /* > * Register switch for ARMv3 and ARMv4 processors > * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info > @@ -1118,17 +1182,29 @@ vector_addrexcptn: > b vector_addrexcptn > > /*============================================================================= > - * Undefined FIQs > + * FIQ "NMI" handler > *----------------------------------------------------------------------------- > - * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC > - * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg. > - * Basically to switch modes, we *HAVE* to clobber one register... brain > - * damage alert! I don't think that we can execute any code in here in any > - * other mode than FIQ... Ok you can switch to another mode, but you can't > - * get out of that mode without clobbering one register. > + * Handle a FIQ using the SVC stack allowing FIQ act like NMI on x86 > + * systems. > */ > -vector_fiq: > - subs pc, lr, #4 > + vector_stub fiq, FIQ_MODE, 4 > + > + .long __fiq_usr @ 0 (USR_26 / USR_32) > + .long __fiq_svc @ 1 (FIQ_26 / FIQ_32) > + .long __fiq_svc @ 2 (IRQ_26 / IRQ_32) > + .long __fiq_svc @ 3 (SVC_26 / SVC_32) > + .long __fiq_svc @ 4 > + .long __fiq_svc @ 5 > + .long __fiq_svc @ 6 > + .long __fiq_abt @ 7 > + .long __fiq_svc @ 8 > + .long __fiq_svc @ 9 > + .long __fiq_svc @ a > + .long __fiq_svc @ b > + .long __fiq_svc @ c > + .long __fiq_svc @ d > + .long __fiq_svc @ e > + .long __fiq_svc @ f > > .globl vector_fiq_offset > .equ vector_fiq_offset, vector_fiq > diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S > index 2fdf867..0d91ca0 100644 > --- a/arch/arm/kernel/entry-header.S > +++ b/arch/arm/kernel/entry-header.S > @@ -216,6 +216,34 @@ > ldmia sp, {r0 - pc}^ @ load r0 - pc, cpsr > .endm > > + @ > + @ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit > + @ > + @ This macro acts in a similar manner to svc_exit but switches to FIQ > + @ mode to restore the final part of the register state. > + @ > + @ We cannot use the normal svc_exit procedure because that would > + @ clobber spsr_svc (FIQ could be delivered during the first few > + @ instructions of vector_swi meaning its contents have not been > + @ saved anywhere). > + @ > + @ Note that, unlike svc_exit, this macro also does not allow a caller > + @ supplied rpsr. This is because the FIQ exceptions are not re-entrant > + @ and the handlers cannot call into the scheduler (meaning the value > + @ on the stack remains correct). > + @ > + .macro svc_exit_via_fiq > + mov r0, sp > + ldmib r0, {r1 - r14} @ abort is deadly from here onward (it will > + @ clobber state restored below) > + msr cpsr_c, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT > + add r8, r0, #S_PC > + ldr r9, [r0, #S_PSR] > + msr spsr_cxsf, r9 > + ldr r0, [r0, #S_R0] > + ldmia r8, {pc}^ > + .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 > @@ -267,6 +295,25 @@ > rfeia sp! > .endm > > + @ > + @ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit > + @ > + @ For full details see non-Thumb implementation above. > + @ > + .macro svc_exit_via_fiq > + add r0, sp, #S_R2 > + ldr lr, [sp, #S_LR] > + ldr sp, [sp, #S_SP] @ abort is deadly from here onward (it will > + @ clobber state restored below) > + ldmia r0, {r2 - r12} > + mov r1, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT > + msr cpsr_c, r1 > + sub r0, #S_R2 > + add r8, r0, #S_PC > + ldmia r0, {r0 - r1} > + rfeia r8 > + .endm > + > #ifdef CONFIG_CPU_V7M > /* > * Note we don't need to do clrex here as clearing the local monitor is > diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c > index 918875d..1743049 100644 > --- a/arch/arm/kernel/fiq.c > +++ b/arch/arm/kernel/fiq.c > @@ -53,6 +53,7 @@ > }) > > static unsigned long no_fiq_insn; > +static struct pt_regs def_fiq_regs; > > /* Default reacquire function > * - we always relinquish FIQ control > @@ -60,8 +61,15 @@ static unsigned long no_fiq_insn; > */ > static int fiq_def_op(void *ref, int relinquish) > { > - if (!relinquish) > + if (!relinquish) { > + /* Restore default handler and registers */ > + local_fiq_disable(); > + set_fiq_regs(&dfl_fiq_regs); > set_fiq_handler(&no_fiq_insn, sizeof(no_fiq_insn)); > + local_fiq_enable(); > + > + /* FIXME: notify irq controller to standard enable FIQs */ > + } > > return 0; > } > @@ -151,5 +159,6 @@ void __init init_FIQ(int start) > { > unsigned offset = FIQ_OFFSET; > no_fiq_insn = *(unsigned long *)(0xffff0000 + offset); > + get_fiq_regs(&dfl_fiq_regs); > fiq_start = start; > } > diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c > index 84db893d..c031063 100644 > --- a/arch/arm/kernel/setup.c > +++ b/arch/arm/kernel/setup.c > @@ -133,6 +133,7 @@ struct stack { > u32 irq[3]; > u32 abt[3]; > u32 und[3]; > + u32 fiq[3]; > } ____cacheline_aligned; > > #ifndef CONFIG_CPU_V7M > @@ -470,7 +471,10 @@ void notrace cpu_init(void) > "msr cpsr_c, %5\n\t" > "add r14, %0, %6\n\t" > "mov sp, r14\n\t" > - "msr cpsr_c, %7" > + "msr cpsr_c, %7\n\t" > + "add r14, %0, %8\n\t" > + "mov sp, r14\n\t" > + "msr cpsr_c, %9" > : > : "r" (stk), > PLC (PSR_F_BIT | PSR_I_BIT | IRQ_MODE), > @@ -479,6 +483,8 @@ void notrace cpu_init(void) > "I" (offsetof(struct stack, abt[0])), > PLC (PSR_F_BIT | PSR_I_BIT | UND_MODE), > "I" (offsetof(struct stack, und[0])), > + PLC (PSR_F_BIT | PSR_I_BIT | FIQ_MODE), > + "I" (offsetof(struct stack, fiq[0])), > PLC (PSR_F_BIT | PSR_I_BIT | SVC_MODE) > : "r14"); > #endif > diff --git a/arch/arm/kernel/traps.c b/arch/arm/kernel/traps.c > index a447dcc..439138d 100644 > --- a/arch/arm/kernel/traps.c > +++ b/arch/arm/kernel/traps.c > @@ -25,6 +25,7 @@ > #include > #include > #include > +#include > > #include > #include > @@ -461,6 +462,31 @@ die_sig: > } > > /* > + * Handle FIQ similarly to NMI on x86 systems. > + * > + * The runtime environment for NMIs is extremely restrictive > + * (NMIs can pre-empt critical sections meaning almost all locking is > + * forbidden) meaning this default FIQ handling must only be used in > + * circumstances where non-maskability improves robustness, such as > + * watchdog or debug logic. > + * > + * This handler is not appropriate for general purpose use in drivers > + * platform code and can be overrideen using set_fiq_handler. > + */ > +asmlinkage void __exception_irq_entry handle_fiq_as_nmi(struct pt_regs *regs) > +{ > + struct pt_regs *old_regs = set_irq_regs(regs); > + > + nmi_enter(); > + > + /* nop. FIQ handlers for special arch/arm features can be added here. */ > + > + nmi_exit(); > + > + set_irq_regs(old_regs); > +} > + > +/* > * bad_mode handles the impossible case in the vectors. If you see one of > * these, then it's extremely serious, and could mean you have buggy hardware. > * It never returns, and never tries to sync. We hope that we can at least > From mboxrd@z Thu Jan 1 00:00:00 1970 From: daniel.thompson@linaro.org (Daniel Thompson) Date: Tue, 16 Sep 2014 00:00:12 +0100 Subject: [PATCH 3.17-rc4 v6 2/6] arm: fiq: Replace default FIQ handler In-Reply-To: <1410695835-10496-3-git-send-email-daniel.thompson@linaro.org> References: <1410695835-10496-1-git-send-email-daniel.thompson@linaro.org> <1410695835-10496-3-git-send-email-daniel.thompson@linaro.org> Message-ID: <54176F7C.6030908@linaro.org> To: linux-arm-kernel@lists.infradead.org List-Id: linux-arm-kernel.lists.infradead.org On 14/09/14 12:57, Daniel Thompson wrote: > This patch introduces a new default FIQ handler that is structured in a > similar way to the existing ARM exception handler and result in the FIQ > being handled by C code running on the SVC stack (despite this code run > in the FIQ handler is subject to severe limitations with respect to > locking making normal interaction with the kernel impossible). > > This default handler allows concepts that on x86 would be handled using > NMIs to be realized on ARM. > > Credit: > > This patch is a near complete re-write of a patch originally > provided by Anton Vorontsov. Today only a couple of small fragments > survive, however without Anton's work to build from this patch would > not exist. Thanks also to Russell King for spoonfeeding me a variety > of fixes during the review cycle. I've send this patch to the your patch tracker as complete respin (#8150/2). If you'd rather handle it as a follow on patch please let me know and I will prepare it as one. Daniel. > > Signed-off-by: Daniel Thompson > Cc: Russell King > Cc: Catalin Marinas > Acked-by: Nicolas Pitre > --- > arch/arm/kernel/entry-armv.S | 98 +++++++++++++++++++++++++++++++++++++----- > arch/arm/kernel/entry-header.S | 47 ++++++++++++++++++++ > arch/arm/kernel/fiq.c | 11 ++++- > arch/arm/kernel/setup.c | 8 +++- > arch/arm/kernel/traps.c | 26 +++++++++++ > 5 files changed, 177 insertions(+), 13 deletions(-) > > diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S > index 36276cd..859f56c 100644 > --- a/arch/arm/kernel/entry-armv.S > +++ b/arch/arm/kernel/entry-armv.S > @@ -146,7 +146,7 @@ ENDPROC(__und_invalid) > #define SPFIX(code...) > #endif > > - .macro svc_entry, stack_hole=0 > + .macro svc_entry, stack_hole=0, trace=1 > UNWIND(.fnstart ) > UNWIND(.save {r0 - pc} ) > sub sp, sp, #(S_FRAME_SIZE + \stack_hole - 4) > @@ -182,9 +182,11 @@ ENDPROC(__und_invalid) > @ > stmia r7, {r2 - r6} > > + .if \trace > #ifdef CONFIG_TRACE_IRQFLAGS > bl trace_hardirqs_off > #endif > + .endif > .endm > > .align 5 > @@ -295,6 +297,15 @@ __pabt_svc: > ENDPROC(__pabt_svc) > > .align 5 > +__fiq_svc: > + svc_entry trace=0 > + mov r0, sp @ struct pt_regs *regs > + bl handle_fiq_as_nmi > + svc_exit_via_fiq > + UNWIND(.fnend ) > +ENDPROC(__fiq_svc) > + > + .align 5 > .LCcralign: > .word cr_alignment > #ifdef MULTI_DABORT > @@ -305,6 +316,46 @@ ENDPROC(__pabt_svc) > .word fp_enter > > /* > + * Abort mode handlers > + */ > + > +@ > +@ Taking a FIQ in abort mode is similar to taking a FIQ in SVC mode > +@ and reuses the same macros. However in abort mode we must also > +@ save/restore lr_abt and spsr_abt to make nested aborts safe. > +@ > + .align 5 > +__fiq_abt: > + svc_entry trace=0 > + > + ARM( msr cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + mov r1, lr @ Save lr_abt > + mrs r2, spsr @ Save spsr_abt, abort is now safe > + ARM( msr cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + stmfd sp!, {r1 - r2} > + > + add r0, sp, #8 @ struct pt_regs *regs > + bl handle_fiq_as_nmi > + > + ldmfd sp!, {r1 - r2} > + ARM( msr cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #ABT_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + mov lr, r1 @ Restore lr_abt, abort is unsafe > + msr spsr_cxsf, r2 @ Restore spsr_abt > + ARM( msr cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( mov r0, #SVC_MODE | PSR_I_BIT | PSR_F_BIT ) > + THUMB( msr cpsr_c, r0 ) > + > + svc_exit_via_fiq > + UNWIND(.fnend ) > +ENDPROC(__fiq_abt) > + > +/* > * User mode handlers > * > * EABI note: sp_svc is always 64-bit aligned here, so should S_FRAME_SIZE > @@ -314,7 +365,7 @@ ENDPROC(__pabt_svc) > #error "sizeof(struct pt_regs) must be a multiple of 8" > #endif > > - .macro usr_entry > + .macro usr_entry, trace=1 > UNWIND(.fnstart ) > UNWIND(.cantunwind ) @ don't unwind the user space > sub sp, sp, #S_FRAME_SIZE > @@ -351,10 +402,12 @@ ENDPROC(__pabt_svc) > @ > zero_fp > > + .if \trace > #ifdef CONFIG_IRQSOFF_TRACER > bl trace_hardirqs_off > #endif > ct_user_exit save = 0 > + .endif > .endm > > .macro kuser_cmpxchg_check > @@ -683,6 +736,17 @@ ENTRY(ret_from_exception) > ENDPROC(__pabt_usr) > ENDPROC(ret_from_exception) > > + .align 5 > +__fiq_usr: > + usr_entry trace=0 > + kuser_cmpxchg_check > + mov r0, sp @ struct pt_regs *regs > + bl handle_fiq_as_nmi > + get_thread_info tsk > + restore_user_regs fast = 0, offset = 0 > + UNWIND(.fnend ) > +ENDPROC(__fiq_usr) > + > /* > * Register switch for ARMv3 and ARMv4 processors > * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info > @@ -1118,17 +1182,29 @@ vector_addrexcptn: > b vector_addrexcptn > > /*============================================================================= > - * Undefined FIQs > + * FIQ "NMI" handler > *----------------------------------------------------------------------------- > - * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC > - * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg. > - * Basically to switch modes, we *HAVE* to clobber one register... brain > - * damage alert! I don't think that we can execute any code in here in any > - * other mode than FIQ... Ok you can switch to another mode, but you can't > - * get out of that mode without clobbering one register. > + * Handle a FIQ using the SVC stack allowing FIQ act like NMI on x86 > + * systems. > */ > -vector_fiq: > - subs pc, lr, #4 > + vector_stub fiq, FIQ_MODE, 4 > + > + .long __fiq_usr @ 0 (USR_26 / USR_32) > + .long __fiq_svc @ 1 (FIQ_26 / FIQ_32) > + .long __fiq_svc @ 2 (IRQ_26 / IRQ_32) > + .long __fiq_svc @ 3 (SVC_26 / SVC_32) > + .long __fiq_svc @ 4 > + .long __fiq_svc @ 5 > + .long __fiq_svc @ 6 > + .long __fiq_abt @ 7 > + .long __fiq_svc @ 8 > + .long __fiq_svc @ 9 > + .long __fiq_svc @ a > + .long __fiq_svc @ b > + .long __fiq_svc @ c > + .long __fiq_svc @ d > + .long __fiq_svc @ e > + .long __fiq_svc @ f > > .globl vector_fiq_offset > .equ vector_fiq_offset, vector_fiq > diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S > index 2fdf867..0d91ca0 100644 > --- a/arch/arm/kernel/entry-header.S > +++ b/arch/arm/kernel/entry-header.S > @@ -216,6 +216,34 @@ > ldmia sp, {r0 - pc}^ @ load r0 - pc, cpsr > .endm > > + @ > + @ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit > + @ > + @ This macro acts in a similar manner to svc_exit but switches to FIQ > + @ mode to restore the final part of the register state. > + @ > + @ We cannot use the normal svc_exit procedure because that would > + @ clobber spsr_svc (FIQ could be delivered during the first few > + @ instructions of vector_swi meaning its contents have not been > + @ saved anywhere). > + @ > + @ Note that, unlike svc_exit, this macro also does not allow a caller > + @ supplied rpsr. This is because the FIQ exceptions are not re-entrant > + @ and the handlers cannot call into the scheduler (meaning the value > + @ on the stack remains correct). > + @ > + .macro svc_exit_via_fiq > + mov r0, sp > + ldmib r0, {r1 - r14} @ abort is deadly from here onward (it will > + @ clobber state restored below) > + msr cpsr_c, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT > + add r8, r0, #S_PC > + ldr r9, [r0, #S_PSR] > + msr spsr_cxsf, r9 > + ldr r0, [r0, #S_R0] > + ldmia r8, {pc}^ > + .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 > @@ -267,6 +295,25 @@ > rfeia sp! > .endm > > + @ > + @ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit > + @ > + @ For full details see non-Thumb implementation above. > + @ > + .macro svc_exit_via_fiq > + add r0, sp, #S_R2 > + ldr lr, [sp, #S_LR] > + ldr sp, [sp, #S_SP] @ abort is deadly from here onward (it will > + @ clobber state restored below) > + ldmia r0, {r2 - r12} > + mov r1, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT > + msr cpsr_c, r1 > + sub r0, #S_R2 > + add r8, r0, #S_PC > + ldmia r0, {r0 - r1} > + rfeia r8 > + .endm > + > #ifdef CONFIG_CPU_V7M > /* > * Note we don't need to do clrex here as clearing the local monitor is > diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c > index 918875d..1743049 100644 > --- a/arch/arm/kernel/fiq.c > +++ b/arch/arm/kernel/fiq.c > @@ -53,6 +53,7 @@ > }) > > static unsigned long no_fiq_insn; > +static struct pt_regs def_fiq_regs; > > /* Default reacquire function > * - we always relinquish FIQ control > @@ -60,8 +61,15 @@ static unsigned long no_fiq_insn; > */ > static int fiq_def_op(void *ref, int relinquish) > { > - if (!relinquish) > + if (!relinquish) { > + /* Restore default handler and registers */ > + local_fiq_disable(); > + set_fiq_regs(&dfl_fiq_regs); > set_fiq_handler(&no_fiq_insn, sizeof(no_fiq_insn)); > + local_fiq_enable(); > + > + /* FIXME: notify irq controller to standard enable FIQs */ > + } > > return 0; > } > @@ -151,5 +159,6 @@ void __init init_FIQ(int start) > { > unsigned offset = FIQ_OFFSET; > no_fiq_insn = *(unsigned long *)(0xffff0000 + offset); > + get_fiq_regs(&dfl_fiq_regs); > fiq_start = start; > } > diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c > index 84db893d..c031063 100644 > --- a/arch/arm/kernel/setup.c > +++ b/arch/arm/kernel/setup.c > @@ -133,6 +133,7 @@ struct stack { > u32 irq[3]; > u32 abt[3]; > u32 und[3]; > + u32 fiq[3]; > } ____cacheline_aligned; > > #ifndef CONFIG_CPU_V7M > @@ -470,7 +471,10 @@ void notrace cpu_init(void) > "msr cpsr_c, %5\n\t" > "add r14, %0, %6\n\t" > "mov sp, r14\n\t" > - "msr cpsr_c, %7" > + "msr cpsr_c, %7\n\t" > + "add r14, %0, %8\n\t" > + "mov sp, r14\n\t" > + "msr cpsr_c, %9" > : > : "r" (stk), > PLC (PSR_F_BIT | PSR_I_BIT | IRQ_MODE), > @@ -479,6 +483,8 @@ void notrace cpu_init(void) > "I" (offsetof(struct stack, abt[0])), > PLC (PSR_F_BIT | PSR_I_BIT | UND_MODE), > "I" (offsetof(struct stack, und[0])), > + PLC (PSR_F_BIT | PSR_I_BIT | FIQ_MODE), > + "I" (offsetof(struct stack, fiq[0])), > PLC (PSR_F_BIT | PSR_I_BIT | SVC_MODE) > : "r14"); > #endif > diff --git a/arch/arm/kernel/traps.c b/arch/arm/kernel/traps.c > index a447dcc..439138d 100644 > --- a/arch/arm/kernel/traps.c > +++ b/arch/arm/kernel/traps.c > @@ -25,6 +25,7 @@ > #include > #include > #include > +#include > > #include > #include > @@ -461,6 +462,31 @@ die_sig: > } > > /* > + * Handle FIQ similarly to NMI on x86 systems. > + * > + * The runtime environment for NMIs is extremely restrictive > + * (NMIs can pre-empt critical sections meaning almost all locking is > + * forbidden) meaning this default FIQ handling must only be used in > + * circumstances where non-maskability improves robustness, such as > + * watchdog or debug logic. > + * > + * This handler is not appropriate for general purpose use in drivers > + * platform code and can be overrideen using set_fiq_handler. > + */ > +asmlinkage void __exception_irq_entry handle_fiq_as_nmi(struct pt_regs *regs) > +{ > + struct pt_regs *old_regs = set_irq_regs(regs); > + > + nmi_enter(); > + > + /* nop. FIQ handlers for special arch/arm features can be added here. */ > + > + nmi_exit(); > + > + set_irq_regs(old_regs); > +} > + > +/* > * bad_mode handles the impossible case in the vectors. If you see one of > * these, then it's extremely serious, and could mean you have buggy hardware. > * It never returns, and never tries to sync. We hope that we can at least >