From c88823612d6ff69a64a86456eb56a9576911466b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Lothar=20Wa=C3=9Fmann?= <LW@KARO-electronics.de>
Date: Thu, 8 Jun 2017 09:52:33 +0200
Subject: arm: adjust PC displayed in exception handlers to point to the
 failing instruction
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Adjust the program counter register to point to the failing
instruction depending on the exeption type.
This makes it easier to localize the offending instruction leading to
a fatal exception.

Signed-off-by: Lothar Waßmann <LW@KARO-electronics.de>

diff --git a/arch/arm/lib/interrupts.c b/arch/arm/lib/interrupts.c
index 066c172..80869ad 100644
--- a/arch/arm/lib/interrupts.c
+++ b/arch/arm/lib/interrupts.c
@@ -93,10 +93,18 @@ void show_regs (struct pt_regs *regs)
 		thumb_mode (regs) ? " (T)" : "");
 }
 
+/* fixup PC to point to the instruction leading to the exception */
+static inline void fixup_pc(struct pt_regs *regs, int offset)
+{
+	uint32_t pc = instruction_pointer(regs) + offset;
+	regs->ARM_pc = pc | (regs->ARM_pc & PCMASK);
+}
+
 void do_undefined_instruction (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("undefined instruction\n");
+	fixup_pc(pt_regs, -4);
 	show_regs (pt_regs);
 	bad_mode ();
 }
@@ -105,6 +113,7 @@ void do_software_interrupt (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("software interrupt\n");
+	fixup_pc(pt_regs, -4);
 	show_regs (pt_regs);
 	bad_mode ();
 }
@@ -113,6 +122,7 @@ void do_prefetch_abort (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("prefetch abort\n");
+	fixup_pc(pt_regs, -8);
 	show_regs (pt_regs);
 	bad_mode ();
 }
@@ -121,6 +131,7 @@ void do_data_abort (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("data abort\n");
+	fixup_pc(pt_regs, -8);
 	show_regs (pt_regs);
 	bad_mode ();
 }
@@ -129,6 +140,7 @@ void do_not_used (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("not used\n");
+	fixup_pc(pt_regs, -8);
 	show_regs (pt_regs);
 	bad_mode ();
 }
@@ -137,6 +149,7 @@ void do_fiq (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("fast interrupt request\n");
+	fixup_pc(pt_regs, -8);
 	show_regs (pt_regs);
 	bad_mode ();
 }
@@ -145,6 +158,7 @@ void do_irq (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
 	printf ("interrupt request\n");
+	fixup_pc(pt_regs, -8);
 	show_regs (pt_regs);
 	bad_mode ();
 }
-- 
cgit v0.10.2