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

25 Oct 2021

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@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


    