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
