From f2fe7e2167db0ef4071aceb952ba5c385f94aae8 Mon Sep 17 00:00:00 2001
From: Daniel Thompson <daniel.thompson@linaro.org>
Date: Mon, 19 Jan 2015 15:45:42 +0000
Subject: arm: kgdb: fiq support

On SMP systems it is necessary to use FIQ to bring other CPUs to a halt
when the system stops.

Signed-off-by: Daniel Thompson <daniel.thompson@linaro.org>
---
 arch/arm/Kconfig.debug      | 15 +++++++++++++++
 arch/arm/include/asm/kgdb.h |  6 ++++++
 arch/arm/kernel/kgdb.c      | 24 ++++++++++++++++++------
 arch/arm/kernel/traps.c     |  4 ++++
 4 files changed, 43 insertions(+), 6 deletions(-)

diff --git a/arch/arm/Kconfig.debug b/arch/arm/Kconfig.debug
index 5ddd4906f7a7..8fc2963e3823 100644
--- a/arch/arm/Kconfig.debug
+++ b/arch/arm/Kconfig.debug
@@ -2,6 +2,21 @@ menu "Kernel hacking"
 
 source "lib/Kconfig.debug"
 
+config KGDB_FIQ
+	bool "KGDB FIQ support"
+	depends on KGDB && MIGHT_HAVE_FIQ
+	help
+	  The FIQ debugger may be used to debug situations when the
+	  kernel stuck in uninterruptable sections, e.g. the kernel
+	  infinitely loops or deadlocked in an interrupt or with
+	  interrupts disabled.
+
+	  By default KGDB FIQ is disabled at runtime, but can be enabled
+	  by setting the console to ttyNMI0 (and choosing an underlying
+	  serial port using kgdboc)
+
+	  If unsure, say N.
+
 config ARM_PTDUMP
 	bool "Export kernel pagetable layout to userspace via debugfs"
 	depends on DEBUG_KERNEL
diff --git a/arch/arm/include/asm/kgdb.h b/arch/arm/include/asm/kgdb.h
index 0a9d5dd93294..58cdffb0f5ca 100644
--- a/arch/arm/include/asm/kgdb.h
+++ b/arch/arm/include/asm/kgdb.h
@@ -48,6 +48,12 @@ static inline void arch_kgdb_breakpoint(void)
 extern void kgdb_handle_bus_error(void);
 extern int kgdb_fault_expected;
 
+#ifdef CONFIG_KGDB_FIQ
+extern void kgdb_handle_fiq(struct pt_regs *regs);
+#else
+static inline void kgdb_handle_fiq(struct pt_regs *regs) {}
+#endif
+
 #endif /* !__ASSEMBLY__ */
 
 /*
diff --git a/arch/arm/kernel/kgdb.c b/arch/arm/kernel/kgdb.c
index 07db2f8a1b45..fe9dc24567b2 100644
--- a/arch/arm/kernel/kgdb.c
+++ b/arch/arm/kernel/kgdb.c
@@ -179,14 +179,26 @@ static struct undef_hook kgdb_compiled_brkpt_hook = {
 
 static void kgdb_call_nmi_hook(void *ignored)
 {
-       kgdb_nmicallback(raw_smp_processor_id(), get_irq_regs());
+	kgdb_nmicallback(raw_smp_processor_id(), get_irq_regs());
 }
 
 void kgdb_roundup_cpus(unsigned long flags)
 {
-       local_irq_enable();
-       smp_call_function(kgdb_call_nmi_hook, NULL, 0);
-       local_irq_disable();
+#if defined CONFIG_KGDB_FIQ && defined CONFIG_SMP
+	struct cpumask mask;
+
+	if (in_nmi()) {
+		cpumask_copy(&mask, cpu_online_mask);
+		cpumask_clear_cpu(raw_smp_processor_id(), &mask);
+		if (!cpumask_empty(&mask))
+			send_nmi_ipi_mask(&mask);
+		return;
+	}
+#endif
+
+	local_irq_enable();
+	smp_call_function(kgdb_call_nmi_hook, NULL, 0);
+	local_irq_disable();
 }
 
 static int __kgdb_notify(struct die_args *args, unsigned long cmd)
@@ -281,8 +293,8 @@ int kgdb_arch_remove_breakpoint(struct kgdb_bkpt *bpt)
  */
 struct kgdb_arch arch_kgdb_ops = {
 #ifndef __ARMEB__
-	.gdb_bpt_instr		= {0xfe, 0xde, 0xff, 0xe7}
+	.gdb_bpt_instr		= {0xfe, 0xde, 0xff, 0xe7},
 #else /* ! __ARMEB__ */
-	.gdb_bpt_instr		= {0xe7, 0xff, 0xde, 0xfe}
+	.gdb_bpt_instr		= {0xe7, 0xff, 0xde, 0xfe},
 #endif
 };
diff --git a/arch/arm/kernel/traps.c b/arch/arm/kernel/traps.c
index 7b697971c309..d158643a965a 100644
--- a/arch/arm/kernel/traps.c
+++ b/arch/arm/kernel/traps.c
@@ -28,6 +28,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/irqchip/arm-gic.h>
+#include <linux/kgdb.h>
 
 #include <linux/atomic.h>
 #include <asm/cacheflush.h>
@@ -490,6 +491,9 @@ asmlinkage void __exception_irq_entry handle_fiq_as_nmi(struct pt_regs *regs)
 	if (irqret == IRQ_NONE) {
 #ifdef CONFIG_SMP
 		ipi_cpu_backtrace(regs);
+#endif
+#ifdef CONFIG_KGDB
+		kgdb_nmicallback(raw_smp_processor_id(), regs);
 #endif
 	}
 
-- 
cgit v1.2.3