[RFC PATCH 5/5] Fix PC adjustment logic in exception handlers

[Jim Posen]

The link register is at a different offset depending on processor
mode, so ensure it applies the correct adjustment.

Signed-off-by: Jim Posen <jim.posen at gmail.com>
---

 arch/arm/include/asm/u-boot-arm.h | 14 +++++++-------
 arch/arm/lib/interrupts.c         | 26 +++++++++++++-------------
 arch/arm/lib/vectors.S            |  4 +++-
 3 files changed, 23 insertions(+), 21 deletions(-)

diff --git a/arch/arm/include/asm/u-boot-arm.h b/arch/arm/include/asm/u-boot-arm.h
index 0b93cc48c5..6ebefa7c86 100644
--- a/arch/arm/include/asm/u-boot-arm.h
+++ b/arch/arm/include/asm/u-boot-arm.h
@@ -41,17 +41,17 @@ int	board_init(void);
 struct pt_regs;
 
 void bad_mode(void);
-void do_undefined_instruction(struct pt_regs *pt_regs);
-void do_software_interrupt(struct pt_regs *pt_regs);
-void do_prefetch_abort(struct pt_regs *pt_regs);
-void do_data_abort(struct pt_regs *pt_regs);
-void do_not_used(struct pt_regs *pt_regs);
+void do_undefined_instruction(struct pt_regs *pt_regs, bool hyp_mode);
+void do_software_interrupt(struct pt_regs *pt_regs, bool hyp_mode);
+void do_prefetch_abort(struct pt_regs *pt_regs, bool hyp_mode);
+void do_data_abort(struct pt_regs *pt_regs, bool hyp_mode);
+void do_not_used(struct pt_regs *pt_regs, bool hyp_mode);
 #ifdef CONFIG_ARM64
 void do_fiq(struct pt_regs *pt_regs, unsigned int esr);
 void do_irq(struct pt_regs *pt_regs, unsigned int esr);
 #else
-void do_fiq(struct pt_regs *pt_regs);
-void do_irq(struct pt_regs *pt_regswq);
+void do_fiq(struct pt_regs *pt_regs, bool hyp_mode);
+void do_irq(struct pt_regs *pt_regswq, bool hyp_mode);
 #endif
 
 void reset_misc(void);
diff --git a/arch/arm/lib/interrupts.c b/arch/arm/lib/interrupts.c
index 6dc27d1d58..498401cdc5 100644
--- a/arch/arm/lib/interrupts.c
+++ b/arch/arm/lib/interrupts.c
@@ -135,17 +135,17 @@ static inline void fixup_pc(struct pt_regs *regs, int offset)
 	regs->ARM_pc = pc | (regs->ARM_pc & PCMASK);
 }
 
-void do_undefined_instruction (struct pt_regs *pt_regs)
+void do_undefined_instruction(struct pt_regs *pt_regs, bool hyp_mode)
 {
 	efi_restore_gd();
 	printf ("undefined instruction\n");
-	fixup_pc(pt_regs, -4);
+	fixup_pc(pt_regs, hyp_mode ? 0 : thumb_mode(regs) ? -2 : -4);
 	show_regs (pt_regs);
 	show_efi_loaded_images(pt_regs);
 	bad_mode ();
 }
 
-void do_software_interrupt (struct pt_regs *pt_regs)
+void do_software_interrupt(struct pt_regs *pt_regs, bool _hyp_mode)
 {
 	efi_restore_gd();
 	printf ("software interrupt\n");
@@ -155,51 +155,51 @@ void do_software_interrupt (struct pt_regs *pt_regs)
 	bad_mode ();
 }
 
-void do_prefetch_abort (struct pt_regs *pt_regs)
+void do_prefetch_abort(struct pt_regs *pt_regs, bool hyp_mode)
 {
 	efi_restore_gd();
 	printf ("prefetch abort\n");
-	fixup_pc(pt_regs, -8);
+	fixup_pc(pt_regs, hyp_mode ? 0 : -4);
 	show_regs (pt_regs);
 	show_efi_loaded_images(pt_regs);
 	bad_mode ();
 }
 
-void do_data_abort (struct pt_regs *pt_regs)
+void do_data_abort(struct pt_regs *pt_regs, bool hyp_mode)
 {
 	efi_restore_gd();
 	printf ("data abort\n");
-	fixup_pc(pt_regs, -8);
+	fixup_pc(pt_regs, hyp_mode ? 0 : -8);
 	show_regs (pt_regs);
 	show_efi_loaded_images(pt_regs);
 	bad_mode ();
 }
 
-void do_not_used (struct pt_regs *pt_regs)
+void do_not_used(struct pt_regs *pt_regs, bool hyp_mode)
 {
 	efi_restore_gd();
 	printf ("not used\n");
-	fixup_pc(pt_regs, -8);
+	fixup_pc(pt_regs, hyp_mode ? 0 : -8);
 	show_regs (pt_regs);
 	show_efi_loaded_images(pt_regs);
 	bad_mode ();
 }
 
-void do_fiq (struct pt_regs *pt_regs)
+void do_fiq(struct pt_regs *pt_regs, bool hyp_mode)
 {
 	efi_restore_gd();
 	printf ("fast interrupt request\n");
-	fixup_pc(pt_regs, -8);
+	fixup_pc(pt_regs, hyp_mode ? 0 : -4);
 	show_regs (pt_regs);
 	show_efi_loaded_images(pt_regs);
 	bad_mode ();
 }
 
-void do_irq (struct pt_regs *pt_regs)
+void do_irq(struct pt_regs *pt_regs, bool hyp_mode)
 {
 	efi_restore_gd();
 	printf ("interrupt request\n");
-	fixup_pc(pt_regs, -8);
+	fixup_pc(pt_regs, hyp_mode ? 0 : -4);
 	show_regs (pt_regs);
 	show_efi_loaded_images(pt_regs);
 	bad_mode ();
diff --git a/arch/arm/lib/vectors.S b/arch/arm/lib/vectors.S
index a36e3b7a43..dff2155dad 100644
--- a/arch/arm/lib/vectors.S
+++ b/arch/arm/lib/vectors.S
@@ -239,6 +239,7 @@ IRQ_STACK_START_IN:
 	mov	r1, lr
 	stmia	r5, {r0 - r3}	@ save sp_SVC, lr_SVC, pc, cpsr
 	mov	r0, sp		@ save current stack into r0 (param register)
+	mov	r1, #0      @ set r1 param to 0, not hypervisor mode
 	.endm
 
 	.macro get_bad_stack
@@ -321,7 +322,8 @@ fiq:
 	str	r1, [sp, #S_PC]
 	str	r2, [sp, #S_PSR]
 	str	r0, [sp, #S_OLD_R0]
-	mov	r0, sp
+	mov	r0, sp		@ set r0 param to struct pt_regs on stack
+	mov	r1, #1      @ set r1 param to 1, hypervisor mode
 	.endm
 
 hyp_undefined_instruction:
-- 
2.25.1