diff mbox

[v10,03/19] arm: fiq: Replace default FIQ handler

Message ID 1408466769-20004-4-git-send-email-daniel.thompson@linaro.org
State New
Headers show

Commit Message

Daniel Thompson Aug. 19, 2014, 4:45 p.m. UTC
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.

Signed-off-by: Daniel Thompson <daniel.thompson@linaro.org>
Cc: Russell King <linux@arm.linux.org.uk>
Cc: Nicolas Pitre <nico@linaro.org>
Cc: Catalin Marinas <catalin.marinas@arm.com>
---
 arch/arm/include/asm/fiq.h   |   1 +
 arch/arm/kernel/entry-armv.S | 129 +++++++++++++++++++++++++++++++++++++++----
 arch/arm/kernel/fiq.c        |  17 ++++++
 arch/arm/kernel/setup.c      |   8 ++-
 4 files changed, 144 insertions(+), 11 deletions(-)

Comments

Russell King - ARM Linux Aug. 19, 2014, 5:37 p.m. UTC | #1
On Tue, Aug 19, 2014 at 05:45:53PM +0100, Daniel Thompson wrote:
> +int register_fiq_nmi_notifier(struct notifier_block *nb)
> +{
> +	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
> +}
> +
> +asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
> +{
> +	struct pt_regs *old_regs = set_irq_regs(regs);
> +
> +	nmi_enter();
> +	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
> +	nmi_exit();
> +	set_irq_regs(old_regs);
> +}

Really not happy with this.  What happens if a FIQ occurs while we're
inside register_fiq_nmi_notifier() - more specifically inside
atomic_notifier_chain_register() ?

This is how easy it is to end up with a deadlock-able situation with
FIQs, and it is why I said:

| > 2. Have default trap handler call an RCU notifier chain to allow it to
| >    hook up with "normal" code without any hard coding (kgdb, IPI
| >    handling, etc)
| 
| Maybe... that sounds like it opens up FIQ for general purpose use which
| is something I want to avoid - I've little motivation to ensure that
| everyone plays by the rules.  Given the choice, I'd rather maintain our
| present stance that using FIQs is hard and requires a lot of thought.

You've just proven my point.

So, what I want is to make FIQs hard to use.  No notifier chain at all
please, people can't be trusted to know all the details (even if they
do know the details, it's just been proven that it's incredibly difficult
to do it right.)

What I instead want to see is that users get added to the above code
fragment with #ifdef's around the calls - that way ensuring that people
have to modify this file, and therefore /should/ be copying me with
their patches.  That means less chance for someone to sneak a change
in by merely calling some register function without first having to
copy this mailing list.
Daniel Thompson Aug. 19, 2014, 6:12 p.m. UTC | #2
On 19/08/14 18:37, Russell King - ARM Linux wrote:
> On Tue, Aug 19, 2014 at 05:45:53PM +0100, Daniel Thompson wrote:
>> +int register_fiq_nmi_notifier(struct notifier_block *nb)
>> +{
>> +	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
>> +}
>> +
>> +asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
>> +{
>> +	struct pt_regs *old_regs = set_irq_regs(regs);
>> +
>> +	nmi_enter();
>> +	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
>> +	nmi_exit();
>> +	set_irq_regs(old_regs);
>> +}
> 
> Really not happy with this.  What happens if a FIQ occurs while we're
> inside register_fiq_nmi_notifier() - more specifically inside
> atomic_notifier_chain_register() ?

Should depend on which side of the rcu update we're on.


> This is how easy it is to end up with a deadlock-able situation with
> FIQs, and it is why I said:
> 
> | > 2. Have default trap handler call an RCU notifier chain to allow it to
> | >    hook up with "normal" code without any hard coding (kgdb, IPI
> | >    handling, etc)
> | 
> | Maybe... that sounds like it opens up FIQ for general purpose use which
> | is something I want to avoid - I've little motivation to ensure that
> | everyone plays by the rules.  Given the choice, I'd rather maintain our
> | present stance that using FIQs is hard and requires a lot of thought.
> 
> You've just proven my point.
> 
> So, what I want is to make FIQs hard to use.  No notifier chain at all
> please, people can't be trusted to know all the details (even if they
> do know the details, it's just been proven that it's incredibly difficult
> to do it right.)

For what its worth the reason I stuck to the notifier idea was that
there has to be some dynamic registration; the gic is registering
something to clear IPIs. Once I had dynamic registration notifiers felt
most elegant.

However, the dynamic registration requirement is in fact very basic
since and can be registered by function pointer since only irqchip
instance is responsible for IPIs. It might even be possible to hardcode
direct calls into the gic driver and have the gic driver maintain a flag.


> What I instead want to see is that users get added to the above code
> fragment with #ifdef's around the calls - that way ensuring that people
> have to modify this file, and therefore /should/ be copying me with
> their patches.  That means less chance for someone to sneak a change
> in by merely calling some register function without first having to
> copy this mailing list.

I can do this.

Note that I'm about to go and live in a field for a week so there will
be a bit of a delay since the field interferes substantially with
hacking time.
Russell King - ARM Linux Aug. 28, 2014, 3:01 p.m. UTC | #3
On Tue, Aug 19, 2014 at 07:12:07PM +0100, Daniel Thompson wrote:
> On 19/08/14 18:37, Russell King - ARM Linux wrote:
> > On Tue, Aug 19, 2014 at 05:45:53PM +0100, Daniel Thompson wrote:
> >> +int register_fiq_nmi_notifier(struct notifier_block *nb)
> >> +{
> >> +	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
> >> +}
> >> +
> >> +asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
> >> +{
> >> +	struct pt_regs *old_regs = set_irq_regs(regs);
> >> +
> >> +	nmi_enter();
> >> +	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
> >> +	nmi_exit();
> >> +	set_irq_regs(old_regs);
> >> +}
> > 
> > Really not happy with this.  What happens if a FIQ occurs while we're
> > inside register_fiq_nmi_notifier() - more specifically inside
> > atomic_notifier_chain_register() ?
> 
> Should depend on which side of the rcu update we're on.

I just asked Paul McKenney, our RCU expert... essentially, yes, RCU
stuff itself is safe in this context.  However, RCU stuff can call into
lockdep if lockdep is configured, and there are questions over lockdep.

There's some things which can be done to reduce the lockdep exposure
to it, such as ensuring that rcu_read_lock() is first called outside
of FIQ context.

There's concerns with whether either printk() in check_flags() could
be reached too (flags there should always indicate that IRQs were
disabled, so that reduces down to a question about just the first
printk() there.)

There's also the very_verbose() stuff for RCU lockdep classes which
Paul says must not be enabled.

Lastly, Paul isn't a lockdep expert, but he sees nothing that prevents
lockdep doing the deadlock checking as a result of the above call.

So... this coupled with my feeling that notifiers make it too easy for
unreviewed code to be hooked into this path, I'm fairly sure that we
don't want to be calling atomic notifier chains from FIQ context.
Paul E. McKenney Aug. 28, 2014, 3:43 p.m. UTC | #4
On Thu, Aug 28, 2014 at 04:01:12PM +0100, Russell King - ARM Linux wrote:
> On Tue, Aug 19, 2014 at 07:12:07PM +0100, Daniel Thompson wrote:
> > On 19/08/14 18:37, Russell King - ARM Linux wrote:
> > > On Tue, Aug 19, 2014 at 05:45:53PM +0100, Daniel Thompson wrote:
> > >> +int register_fiq_nmi_notifier(struct notifier_block *nb)
> > >> +{
> > >> +	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
> > >> +}
> > >> +
> > >> +asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
> > >> +{
> > >> +	struct pt_regs *old_regs = set_irq_regs(regs);
> > >> +
> > >> +	nmi_enter();
> > >> +	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
> > >> +	nmi_exit();
> > >> +	set_irq_regs(old_regs);
> > >> +}
> > > 
> > > Really not happy with this.  What happens if a FIQ occurs while we're
> > > inside register_fiq_nmi_notifier() - more specifically inside
> > > atomic_notifier_chain_register() ?
> > 
> > Should depend on which side of the rcu update we're on.
> 
> I just asked Paul McKenney, our RCU expert... essentially, yes, RCU
> stuff itself is safe in this context.  However, RCU stuff can call into
> lockdep if lockdep is configured, and there are questions over lockdep.
> 
> There's some things which can be done to reduce the lockdep exposure
> to it, such as ensuring that rcu_read_lock() is first called outside
> of FIQ context.
> 
> There's concerns with whether either printk() in check_flags() could
> be reached too (flags there should always indicate that IRQs were
> disabled, so that reduces down to a question about just the first
> printk() there.)
> 
> There's also the very_verbose() stuff for RCU lockdep classes which
> Paul says must not be enabled.
> 
> Lastly, Paul isn't a lockdep expert, but he sees nothing that prevents
> lockdep doing the deadlock checking as a result of the above call.
> 
> So... this coupled with my feeling that notifiers make it too easy for
> unreviewed code to be hooked into this path, I'm fairly sure that we
> don't want to be calling atomic notifier chains from FIQ context.

In the worst case, it would be possible to create a parallel notifier
that was intended for use from NMI.  There would be no need for
rcu_read_lock() in that case, we would instead be using RCU-sched,
for which NMI handlers are automatically RCU-sched read-side critical
sections.  Instead of synchronize_rcu(), this NMI version would use
synchronize_sched().

But if lockdep works from NMI, then the current notifiers would work
just fine.

							Thanx, Paul
Daniel Thompson Aug. 28, 2014, 3:54 p.m. UTC | #5
On 28/08/14 16:01, Russell King - ARM Linux wrote:
> On Tue, Aug 19, 2014 at 07:12:07PM +0100, Daniel Thompson wrote:
>> On 19/08/14 18:37, Russell King - ARM Linux wrote:
>>> On Tue, Aug 19, 2014 at 05:45:53PM +0100, Daniel Thompson wrote:
>>>> +int register_fiq_nmi_notifier(struct notifier_block *nb)
>>>> +{
>>>> +	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
>>>> +}
>>>> +
>>>> +asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
>>>> +{
>>>> +	struct pt_regs *old_regs = set_irq_regs(regs);
>>>> +
>>>> +	nmi_enter();
>>>> +	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
>>>> +	nmi_exit();
>>>> +	set_irq_regs(old_regs);
>>>> +}
>>>
>>> Really not happy with this.  What happens if a FIQ occurs while we're
>>> inside register_fiq_nmi_notifier() - more specifically inside
>>> atomic_notifier_chain_register() ?
>>
>> Should depend on which side of the rcu update we're on.
> 
> I just asked Paul McKenney, our RCU expert... essentially, yes, RCU
> stuff itself is safe in this context.  However, RCU stuff can call into
> lockdep if lockdep is configured, and there are questions over lockdep.

Thanks for following this up.

I originally formed the opinion RCU was safe from FIQ because it is also
used to manage the NMI notification handlers for x86
(register_nmi_handler) and I understood the runtime constraints on FIQ
to be very similar.

Note that x86 manages the notifiers itself so it uses
list_for_each_entry_rcu() rather atomic_notifier_call_chain() but
nevertheless I think this boils down to the same thing w.r.t. safety
concerns.


> There's some things which can be done to reduce the lockdep exposure
> to it, such as ensuring that rcu_read_lock() is first called outside
> of FIQ context.

lockdep is automatically disabled by calling nmi_enter() so all the
lockdep calls should end up following the early exit path based on
current->lockdep_recursion.


> There's concerns with whether either printk() in check_flags() could
> be reached too (flags there should always indicate that IRQs were
> disabled, so that reduces down to a question about just the first
> printk() there.)
> 
> There's also the very_verbose() stuff for RCU lockdep classes which
> Paul says must not be enabled.
> 
> Lastly, Paul isn't a lockdep expert, but he sees nothing that prevents
> lockdep doing the deadlock checking as a result of the above call.
>
> So... this coupled with my feeling that notifiers make it too easy for
> unreviewed code to be hooked into this path, I'm fairly sure that we
> don't want to be calling atomic notifier chains from FIQ context.
Paul E. McKenney Aug. 28, 2014, 4:15 p.m. UTC | #6
On Thu, Aug 28, 2014 at 04:54:25PM +0100, Daniel Thompson wrote:
> On 28/08/14 16:01, Russell King - ARM Linux wrote:
> > On Tue, Aug 19, 2014 at 07:12:07PM +0100, Daniel Thompson wrote:
> >> On 19/08/14 18:37, Russell King - ARM Linux wrote:
> >>> On Tue, Aug 19, 2014 at 05:45:53PM +0100, Daniel Thompson wrote:
> >>>> +int register_fiq_nmi_notifier(struct notifier_block *nb)
> >>>> +{
> >>>> +	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
> >>>> +}
> >>>> +
> >>>> +asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
> >>>> +{
> >>>> +	struct pt_regs *old_regs = set_irq_regs(regs);
> >>>> +
> >>>> +	nmi_enter();
> >>>> +	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
> >>>> +	nmi_exit();
> >>>> +	set_irq_regs(old_regs);
> >>>> +}
> >>>
> >>> Really not happy with this.  What happens if a FIQ occurs while we're
> >>> inside register_fiq_nmi_notifier() - more specifically inside
> >>> atomic_notifier_chain_register() ?
> >>
> >> Should depend on which side of the rcu update we're on.
> > 
> > I just asked Paul McKenney, our RCU expert... essentially, yes, RCU
> > stuff itself is safe in this context.  However, RCU stuff can call into
> > lockdep if lockdep is configured, and there are questions over lockdep.
> 
> Thanks for following this up.
> 
> I originally formed the opinion RCU was safe from FIQ because it is also
> used to manage the NMI notification handlers for x86
> (register_nmi_handler) and I understood the runtime constraints on FIQ
> to be very similar.
> 
> Note that x86 manages the notifiers itself so it uses
> list_for_each_entry_rcu() rather atomic_notifier_call_chain() but
> nevertheless I think this boils down to the same thing w.r.t. safety
> concerns.
> 
> 
> > There's some things which can be done to reduce the lockdep exposure
> > to it, such as ensuring that rcu_read_lock() is first called outside
> > of FIQ context.
> 
> lockdep is automatically disabled by calling nmi_enter() so all the
> lockdep calls should end up following the early exit path based on
> current->lockdep_recursion.

Ah, that was what I was missing.  Then the notification should be
safe from NMI, so have at it!  ;-)

							Thanx, Paul

> > There's concerns with whether either printk() in check_flags() could
> > be reached too (flags there should always indicate that IRQs were
> > disabled, so that reduces down to a question about just the first
> > printk() there.)
> > 
> > There's also the very_verbose() stuff for RCU lockdep classes which
> > Paul says must not be enabled.
> > 
> > Lastly, Paul isn't a lockdep expert, but he sees nothing that prevents
> > lockdep doing the deadlock checking as a result of the above call.
> >
> > So... this coupled with my feeling that notifiers make it too easy for
> > unreviewed code to be hooked into this path, I'm fairly sure that we
> > don't want to be calling atomic notifier chains from FIQ context.
> 
> 
>
diff mbox

Patch

diff --git a/arch/arm/include/asm/fiq.h b/arch/arm/include/asm/fiq.h
index a25c952..175bfed 100644
--- a/arch/arm/include/asm/fiq.h
+++ b/arch/arm/include/asm/fiq.h
@@ -54,6 +54,7 @@  extern void disable_fiq(int fiq);
 extern int ack_fiq(int fiq);
 extern void eoi_fiq(int fiq);
 extern bool has_fiq(int fiq);
+extern int register_fiq_nmi_notifier(struct notifier_block *nb);
 extern void fiq_register_mapping(int irq, struct fiq_chip *chip);
 
 /* helpers defined in fiqasm.S: */
diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index 36276cd..ef64333 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -79,6 +79,15 @@ 
 #endif
 	.endm
 
+	.macro	fiq_handler
+	ldr	r1, =.LChandle_fiq
+	mov	r0, sp
+	adr	lr, BSYM(9998f)
+	ldr	pc, [r1]
+9998:
+	.endm
+
+
 #ifdef CONFIG_KPROBES
 	.section	.kprobes.text,"ax",%progbits
 #else
@@ -146,7 +155,7 @@  ENDPROC(__und_invalid)
 #define SPFIX(code...)
 #endif
 
-	.macro	svc_entry, stack_hole=0
+	.macro	svc_entry, stack_hole=0, call_trace=1
  UNWIND(.fnstart		)
  UNWIND(.save {r0 - pc}		)
 	sub	sp, sp, #(S_FRAME_SIZE + \stack_hole - 4)
@@ -183,10 +192,35 @@  ENDPROC(__und_invalid)
 	stmia	r7, {r2 - r6}
 
 #ifdef CONFIG_TRACE_IRQFLAGS
+	.if \call_trace
 	bl	trace_hardirqs_off
+	.endif
 #endif
 	.endm
 
+@
+@ svc_exit_via_fiq - similar to 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).
+@
+	.macro  svc_exit_via_fiq, rpsr
+
+	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
+
 	.align	5
 __dabt_svc:
 	svc_entry
@@ -295,6 +329,14 @@  __pabt_svc:
 ENDPROC(__pabt_svc)
 
 	.align	5
+__fiq_svc:
+	svc_entry 0, 0
+	fiq_handler
+	svc_exit_via_fiq r5
+ UNWIND(.fnend		)
+ENDPROC(__fiq_svc)
+
+	.align	5
 .LCcralign:
 	.word	cr_alignment
 #ifdef MULTI_DABORT
@@ -303,6 +345,43 @@  ENDPROC(__pabt_svc)
 #endif
 .LCfp:
 	.word	fp_enter
+.LChandle_fiq:
+#ifdef CONFIG_FIQ
+	.word	fiq_nmi_handler
+#else
+	.word	do_unexp_fiq
+#endif
+
+/*
+ * 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 0, 0
+
+	msr	cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT
+	mov	r0, lr		@ Save lr_abt
+	mrs	r1, spsr	@ Save spsr_abt, abort is now safe
+	msr	cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT
+	push	{r0 - r1}
+
+	fiq_handler
+
+	pop	{r0 - r1}
+	msr	cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT
+	mov	lr, r0		@ Restore lr_abt, abort is unsafe
+	msr	spsr_cxsf, r1	@ Restore spsr_abt
+	msr	cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT
+
+	svc_exit_via_fiq r5
+ UNWIND(.fnend		)
+ENDPROC(__fiq_svc)
 
 /*
  * User mode handlers
@@ -683,6 +762,17 @@  ENTRY(ret_from_exception)
 ENDPROC(__pabt_usr)
 ENDPROC(ret_from_exception)
 
+	.align	5
+__fiq_usr:
+	usr_entry
+	kuser_cmpxchg_check
+	fiq_handler
+	get_thread_info tsk
+	mov	why, #0
+	b	ret_to_user_from_irq
+ 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 +1208,36 @@  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. 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 inappropriate for high performance (fast) interrupt
+ * servicing and can be overridden using set_fiq_handler.
  */
-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/fiq.c b/arch/arm/kernel/fiq.c
index 3ccaa8c..b2bd1c7 100644
--- a/arch/arm/kernel/fiq.c
+++ b/arch/arm/kernel/fiq.c
@@ -46,6 +46,7 @@ 
 
 #include <asm/cacheflush.h>
 #include <asm/cp15.h>
+#include <asm/exception.h>
 #include <asm/fiq.h>
 #include <asm/irq.h>
 #include <asm/traps.h>
@@ -64,6 +65,7 @@  static unsigned long no_fiq_insn;
 static int fiq_start = -1;
 static RADIX_TREE(fiq_data_tree, GFP_KERNEL);
 static DEFINE_MUTEX(fiq_data_mutex);
+static ATOMIC_NOTIFIER_HEAD(fiq_nmi_chain);
 
 /* Default reacquire function
  * - we always relinquish FIQ control
@@ -216,6 +218,21 @@  bool has_fiq(int fiq)
 }
 EXPORT_SYMBOL(has_fiq);
 
+int register_fiq_nmi_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&fiq_nmi_chain, nb);
+}
+
+asmlinkage void __exception_irq_entry fiq_nmi_handler(struct pt_regs *regs)
+{
+	struct pt_regs *old_regs = set_irq_regs(regs);
+
+	nmi_enter();
+	atomic_notifier_call_chain(&fiq_nmi_chain, (unsigned long)regs, NULL);
+	nmi_exit();
+	set_irq_regs(old_regs);
+}
+
 EXPORT_SYMBOL(set_fiq_handler);
 EXPORT_SYMBOL(__set_fiq_regs);	/* defined in fiqasm.S */
 EXPORT_SYMBOL(__get_fiq_regs);	/* defined in fiqasm.S */
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