bsps/powerpc: PPC_EXC_CONFIG_USE_FIXED_HANDLER

Make PPC_EXC_CONFIG_USE_FIXED_HANDLER mandatory for BSPs using
ppc_exc_interrupt().  Pass exception number to bsp_interrupt_dispatch()
to allow processing of decrementer and doorbell exceptions as hypervisor
guest.

Update #3085.

3 files changed, 107 insertions, 179 deletions

diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/irq/irq.c b/c/src/lib/libcpu/powerpc/mpc55xx/irq/irq.c
index 789a19a490..53c30f171e 100644
--- a/c/src/lib/libcpu/powerpc/mpc55xx/irq/irq.c
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/irq/irq.c
@@ -113,9 +113,9 @@ rtems_status_code mpc55xx_interrupt_handler_install(
 	}
 }
 
-static void mpc55xx_interrupt_dispatch(void)
+void bsp_interrupt_dispatch(uintptr_t exception_number)
 {
-	/* Acknowlege interrupt request */
+	/* Acknowledge interrupt request */
 	rtems_vector_number vector = INTC.IACKR.B.INTVEC;
 
 	/* Save machine state and enable external exceptions */
@@ -131,38 +131,10 @@ static void mpc55xx_interrupt_dispatch(void)
 	INTC.EOIR.R = 1;
 }
 
-#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-
-/**
- * @brief External exception handler.
- */
-static int mpc55xx_external_exception_handler( BSP_Exception_frame *frame, unsigned exception_number)
-{
-	mpc55xx_interrupt_dispatch();
-
-	return 0;
-}
-
-#else /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
-
-void bsp_interrupt_dispatch(void)
-{
-	mpc55xx_interrupt_dispatch();
-}
-
-#endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
-
 rtems_status_code bsp_interrupt_facility_initialize(void)
 {
 	rtems_vector_number vector;
 
-#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-	/* Install exception handler */
-	if (ppc_exc_set_handler( ASM_EXT_VECTOR, mpc55xx_external_exception_handler)) {
-		return RTEMS_IO_ERROR;
-	}
-#endif
-
 	/* Initialize interrupt controller */
 
 	/* Disable all interrupts */
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S
index 5b2a1b48e0..69ccb25de0 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S
@@ -16,30 +16,30 @@
 #include <rtems/score/percpu.h>
 #include <bsp/vectors.h>
 
-#define VECTOR_REGISTER r4
-#define SELF_CPU_REGISTER r5
-#define ISR_NEST_REGISTER r6
-#define DISPATCH_LEVEL_REGISTER r7
-#define HANDLER_REGISTER r8
 #define SCRATCH_0_REGISTER r0
 #define SCRATCH_1_REGISTER r3
-#define SCRATCH_2_REGISTER r9
-#define SCRATCH_3_REGISTER r10
-#define SCRATCH_4_REGISTER r11
-#define SCRATCH_5_REGISTER r12
+#define SCRATCH_2_REGISTER r4
+#define SCRATCH_3_REGISTER r5
+#define SCRATCH_4_REGISTER r6
+#define SCRATCH_5_REGISTER r7
+#define SCRATCH_6_REGISTER r8
+#define SCRATCH_7_REGISTER r9
+#define SCRATCH_8_REGISTER r10
+#define SCRATCH_9_REGISTER r11
+#define SCRATCH_10_REGISTER r12
 #define FRAME_REGISTER r14
 
-#define VECTOR_OFFSET GPR4_OFFSET
-#define SELF_CPU_OFFSET GPR5_OFFSET
-#define ISR_NEST_OFFSET GPR6_OFFSET
-#define DISPATCH_LEVEL_OFFSET GPR7_OFFSET
-#define HANDLER_OFFSET GPR8_OFFSET
 #define SCRATCH_0_OFFSET GPR0_OFFSET
 #define SCRATCH_1_OFFSET GPR3_OFFSET
-#define SCRATCH_2_OFFSET GPR9_OFFSET
-#define SCRATCH_3_OFFSET GPR10_OFFSET
-#define SCRATCH_4_OFFSET GPR11_OFFSET
-#define SCRATCH_5_OFFSET GPR12_OFFSET
+#define SCRATCH_2_OFFSET GPR4_OFFSET
+#define SCRATCH_3_OFFSET GPR5_OFFSET
+#define SCRATCH_4_OFFSET GPR6_OFFSET
+#define SCRATCH_5_OFFSET GPR7_OFFSET
+#define SCRATCH_6_OFFSET GPR8_OFFSET
+#define SCRATCH_7_OFFSET GPR9_OFFSET
+#define SCRATCH_8_OFFSET GPR10_OFFSET
+#define SCRATCH_9_OFFSET GPR11_OFFSET
+#define SCRATCH_10_OFFSET GPR12_OFFSET
 #define FRAME_OFFSET PPC_EXC_INTERRUPT_FRAME_OFFSET
 
 #ifdef RTEMS_PROFILING
@@ -54,21 +54,14 @@
 .endm
 #endif /* RTEMS_PROFILING */
 
-#ifdef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-	.global	bsp_interrupt_dispatch
-#endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
-
 	.global	ppc_exc_min_prolog_async_tmpl_normal
 	.global ppc_exc_interrupt
 
 ppc_exc_min_prolog_async_tmpl_normal:
 
 	stwu	r1, -PPC_EXC_INTERRUPT_FRAME_SIZE(r1)
-
-#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-	stw	VECTOR_REGISTER, PPC_EXC_VECTOR_PROLOGUE_OFFSET(r1)
-	li	VECTOR_REGISTER, 0xffff8000
-#endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
+	PPC_REG_STORE	SCRATCH_1_REGISTER, SCRATCH_1_OFFSET(r1)
+	li	SCRATCH_1_REGISTER, 0xffff8000
 
 	/*
 	 * We store the absolute branch target address here.  It will be used
@@ -111,76 +104,51 @@ ppc_exc_interrupt:
 	/* Move frame pointer to non-volatile FRAME_REGISTER */
 	mr	FRAME_REGISTER, r1
 
-	/* Load ISR nest level and thread dispatch disable level */
-	PPC_GPR_STORE	SELF_CPU_REGISTER, SELF_CPU_OFFSET(r1)
-	GET_SELF_CPU_CONTROL	SELF_CPU_REGISTER
-	PPC_GPR_STORE	ISR_NEST_REGISTER, ISR_NEST_OFFSET(r1)
-	lwz	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
-	PPC_GPR_STORE	DISPATCH_LEVEL_REGISTER, DISPATCH_LEVEL_OFFSET(r1)
-	lwz	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
-
-	PPC_GPR_STORE	SCRATCH_0_REGISTER, SCRATCH_0_OFFSET(r1)
-
-#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-#ifdef __SPE__
 	/*
-	 * Save high order part of VECTOR_REGISTER here.  The low order part
-	 * was saved in the minimal prologue.
+	 * Save volatile registers.  The SCRATCH_1_REGISTER has been saved in
+	 * minimum prologue.
 	 */
-	evmergehi	SCRATCH_0_REGISTER, SCRATCH_0_REGISTER, VECTOR_REGISTER
-	stw	SCRATCH_0_REGISTER, VECTOR_OFFSET(r1)
-#endif
-#else /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
-	/* The vector register has no special purpose in this case */
-	PPC_GPR_STORE	VECTOR_REGISTER, VECTOR_OFFSET(r1)
-#endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
-
-	PPC_GPR_STORE	HANDLER_REGISTER, HANDLER_OFFSET(r1)
-
-#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-	/*
-	 * Load the handler address.  Get the handler table index from the
-	 * vector number.  We have to discard the exception type.  Take only
-	 * the least significant five bits (= LAST_VALID_EXC + 1) from the
-	 * vector register.  Multiply by four (= size of function pointer).
-	 */
-	rlwinm	SCRATCH_0_REGISTER, VECTOR_REGISTER, 2, 25, 29
-	lis	HANDLER_REGISTER, ppc_exc_handler_table@h
-	ori	HANDLER_REGISTER, HANDLER_REGISTER, ppc_exc_handler_table@l
-	lwzx	HANDLER_REGISTER, HANDLER_REGISTER, SCRATCH_0_REGISTER
-#endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
-
+	PPC_GPR_STORE	SCRATCH_0_REGISTER, SCRATCH_0_OFFSET(r1)
 #ifdef __powerpc64__
 	PPC_GPR_STORE	r2, GPR2_OFFSET(r1)
 	LA32	r2, .TOC.
 #endif
-	PPC_GPR_STORE	SCRATCH_1_REGISTER, SCRATCH_1_OFFSET(r1)
 	PPC_GPR_STORE	SCRATCH_2_REGISTER, SCRATCH_2_OFFSET(r1)
+	GET_SELF_CPU_CONTROL	SCRATCH_2_REGISTER
 	PPC_GPR_STORE	SCRATCH_3_REGISTER, SCRATCH_3_OFFSET(r1)
 	PPC_GPR_STORE	SCRATCH_4_REGISTER, SCRATCH_4_OFFSET(r1)
 	PPC_GPR_STORE	SCRATCH_5_REGISTER, SCRATCH_5_OFFSET(r1)
+	PPC_GPR_STORE	SCRATCH_6_REGISTER, SCRATCH_6_OFFSET(r1)
+	PPC_GPR_STORE	SCRATCH_7_REGISTER, SCRATCH_7_OFFSET(r1)
+	PPC_GPR_STORE	SCRATCH_8_REGISTER, SCRATCH_8_OFFSET(r1)
+	PPC_GPR_STORE	SCRATCH_9_REGISTER, SCRATCH_9_OFFSET(r1)
+	PPC_GPR_STORE	SCRATCH_10_REGISTER, SCRATCH_10_OFFSET(r1)
+
+	/* Load ISR nest level and thread dispatch disable level */
+	lwz	SCRATCH_3_REGISTER, PER_CPU_ISR_NEST_LEVEL(SCRATCH_2_REGISTER)
+	lwz	SCRATCH_4_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_2_REGISTER)
 
 	/* Save SRR0, SRR1, CR, XER, CTR, and LR */
 	mfsrr0	SCRATCH_0_REGISTER
-	mfsrr1	SCRATCH_1_REGISTER
-	mfcr	SCRATCH_2_REGISTER
-	mfxer	SCRATCH_3_REGISTER
-	mfctr	SCRATCH_4_REGISTER
-	mflr	SCRATCH_5_REGISTER
+	mfsrr1	SCRATCH_5_REGISTER
+	mfcr	SCRATCH_6_REGISTER
+	mfxer	SCRATCH_7_REGISTER
+	mfctr	SCRATCH_8_REGISTER
+	mflr	SCRATCH_9_REGISTER
 	PPC_REG_STORE	SCRATCH_0_REGISTER, SRR0_FRAME_OFFSET(r1)
-	PPC_REG_STORE	SCRATCH_1_REGISTER, SRR1_FRAME_OFFSET(r1)
-	stw	SCRATCH_2_REGISTER, EXC_CR_OFFSET(r1)
-	stw	SCRATCH_3_REGISTER, EXC_XER_OFFSET(r1)
-	PPC_REG_STORE	SCRATCH_4_REGISTER, EXC_CTR_OFFSET(r1)
-	PPC_REG_STORE	SCRATCH_5_REGISTER, EXC_LR_OFFSET(r1)
+	PPC_REG_STORE	SCRATCH_5_REGISTER, SRR1_FRAME_OFFSET(r1)
+	stw	SCRATCH_6_REGISTER, EXC_CR_OFFSET(r1)
+	stw	SCRATCH_7_REGISTER, EXC_XER_OFFSET(r1)
+	PPC_REG_STORE	SCRATCH_8_REGISTER, EXC_CTR_OFFSET(r1)
+	PPC_REG_STORE	SCRATCH_9_REGISTER, EXC_LR_OFFSET(r1)
 
 #ifdef __SPE__
 	/* Save SPEFSCR and ACC */
 	mfspr	SCRATCH_0_REGISTER, FSL_EIS_SPEFSCR
-	evxor	SCRATCH_1_REGISTER, SCRATCH_1_REGISTER, SCRATCH_1_REGISTER
-	evmwumiaa	SCRATCH_1_REGISTER, SCRATCH_1_REGISTER, SCRATCH_1_REGISTER
+	evxor	SCRATCH_5_REGISTER, SCRATCH_5_REGISTER, SCRATCH_5_REGISTER
+	evmwumiaa	SCRATCH_5_REGISTER, SCRATCH_5_REGISTER, SCRATCH_5_REGISTER
 	stw	SCRATCH_0_REGISTER, PPC_EXC_SPEFSCR_OFFSET(r1)
-	evstdd	SCRATCH_1_REGISTER, PPC_EXC_ACC_OFFSET(r1)
+	evstdd	SCRATCH_5_REGISTER, PPC_EXC_ACC_OFFSET(r1)
 #endif
 
 #ifdef PPC_MULTILIB_ALTIVEC
@@ -251,35 +219,22 @@ ppc_exc_interrupt:
 #endif
 
 	/* Increment ISR nest level and thread dispatch disable level */
-	cmpwi	ISR_NEST_REGISTER, 0
+	cmpwi	SCRATCH_3_REGISTER, 0
 #ifdef RTEMS_PROFILING
-	cmpwi	cr2, ISR_NEST_REGISTER, 0
+	cmpwi	cr2, SCRATCH_3_REGISTER, 0
 #endif
-	addi	ISR_NEST_REGISTER, ISR_NEST_REGISTER, 1
-	addi	DISPATCH_LEVEL_REGISTER, DISPATCH_LEVEL_REGISTER, 1
-	stw	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
-	stw	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
+	addi	SCRATCH_3_REGISTER, SCRATCH_3_REGISTER, 1
+	addi	SCRATCH_4_REGISTER, SCRATCH_4_REGISTER, 1
+	stw	SCRATCH_3_REGISTER, PER_CPU_ISR_NEST_LEVEL(SCRATCH_2_REGISTER)
+	stw	SCRATCH_4_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_2_REGISTER)
 
 	/* Switch stack if necessary */
 	mfspr	SCRATCH_0_REGISTER, SPRG1
 	iselgt	r1, r1, SCRATCH_0_REGISTER
 
-#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
-	/*
-	 * Call high level exception handler.
-	 *
-	 * First parameter = exception frame pointer + FRAME_LINK_SPACE
-	 * Second parameter = vector number (r4 is the VECTOR_REGISTER)
-	 */
-	addi	r3, FRAME_REGISTER, FRAME_LINK_SPACE
-	rlwinm	VECTOR_REGISTER, VECTOR_REGISTER, 0, 27, 31
-	mtctr	HANDLER_REGISTER
-	bctrl
-#else /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
 	/* Call fixed high level handler */
 	bl	bsp_interrupt_dispatch
 	PPC64_NOP_FOR_LINKER_TOC_POINTER_RESTORE
-#endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
 
 #ifdef RTEMS_PROFILING
 	/* Update profiling data if necessary */
@@ -293,11 +248,11 @@ ppc_exc_interrupt:
 #endif /* RTEMS_PROFILING */
 
 	/* Load some per-CPU variables */
-	GET_SELF_CPU_CONTROL	SELF_CPU_REGISTER
-	lbz	SCRATCH_0_REGISTER, PER_CPU_DISPATCH_NEEDED(SELF_CPU_REGISTER)
-	lwz	SCRATCH_1_REGISTER, PER_CPU_ISR_DISPATCH_DISABLE(SELF_CPU_REGISTER)
-	lwz	SCRATCH_2_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
-	lwz	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
+	GET_SELF_CPU_CONTROL	SCRATCH_1_REGISTER
+	lbz	SCRATCH_0_REGISTER, PER_CPU_DISPATCH_NEEDED(SCRATCH_1_REGISTER)
+	lwz	SCRATCH_5_REGISTER, PER_CPU_ISR_DISPATCH_DISABLE(SCRATCH_1_REGISTER)
+	lwz	SCRATCH_6_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_1_REGISTER)
+	lwz	SCRATCH_3_REGISTER, PER_CPU_ISR_NEST_LEVEL(SCRATCH_1_REGISTER)
 
 	/*
 	 * Switch back to original stack (FRAME_REGISTER == r1 if we are still
@@ -308,14 +263,14 @@ ppc_exc_interrupt:
 
 	/* Decrement levels and determine thread dispatch state */
 	xori	SCRATCH_0_REGISTER, SCRATCH_0_REGISTER, 1
-	or	SCRATCH_0_REGISTER, SCRATCH_0_REGISTER, SCRATCH_1_REGISTER
-	subi	DISPATCH_LEVEL_REGISTER, SCRATCH_2_REGISTER, 1
-	or.	SCRATCH_0_REGISTER, SCRATCH_0_REGISTER, DISPATCH_LEVEL_REGISTER
-	subi	ISR_NEST_REGISTER, ISR_NEST_REGISTER, 1
+	or	SCRATCH_0_REGISTER, SCRATCH_0_REGISTER, SCRATCH_5_REGISTER
+	subi	SCRATCH_4_REGISTER, SCRATCH_6_REGISTER, 1
+	or.	SCRATCH_0_REGISTER, SCRATCH_0_REGISTER, SCRATCH_4_REGISTER
+	subi	SCRATCH_3_REGISTER, SCRATCH_3_REGISTER, 1
 
 	/* Store thread dispatch disable and ISR nest levels */
-	stw	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
-	stw	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
+	stw	SCRATCH_4_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_1_REGISTER)
+	stw	SCRATCH_3_REGISTER, PER_CPU_ISR_NEST_LEVEL(SCRATCH_1_REGISTER)
 
 	/*
 	 * Check thread dispatch necessary, ISR dispatch disable and thread
@@ -328,11 +283,13 @@ ppc_exc_interrupt:
 
 	/* Set ISR dispatch disable and thread dispatch disable level to one */
 	li	SCRATCH_0_REGISTER, 1
-	stw	SCRATCH_0_REGISTER, PER_CPU_ISR_DISPATCH_DISABLE(SELF_CPU_REGISTER)
-	stw	SCRATCH_0_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
+	stw	SCRATCH_0_REGISTER, PER_CPU_ISR_DISPATCH_DISABLE(SCRATCH_1_REGISTER)
+	stw	SCRATCH_0_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_1_REGISTER)
 
-	/* Call _Thread_Do_dispatch(), this function will enable interrupts */
-	mr	r3, SELF_CPU_REGISTER
+	/*
+	 * Call _Thread_Do_dispatch(), this function will enable interrupts.
+	 * The r3 is SCRATCH_1_REGISTER.
+	 */
 	mfmsr	r4
 	ori	r4, r4, MSR_EE
 	bl	_Thread_Do_dispatch
@@ -341,17 +298,17 @@ ppc_exc_interrupt:
 	/* Disable interrupts */
 	wrteei	0
 
-	/* SELF_CPU_REGISTER is volatile, we must set it again */
-	GET_SELF_CPU_CONTROL	SELF_CPU_REGISTER
+	/* SCRATCH_1_REGISTER is volatile, we must set it again */
+	GET_SELF_CPU_CONTROL	SCRATCH_1_REGISTER
 
 	/* Check if we have to do the thread dispatch again */
-	lbz	SCRATCH_0_REGISTER, PER_CPU_DISPATCH_NEEDED(SELF_CPU_REGISTER)
+	lbz	SCRATCH_0_REGISTER, PER_CPU_DISPATCH_NEEDED(SCRATCH_1_REGISTER)
 	cmpwi	SCRATCH_0_REGISTER, 0
 	bne	.Ldo_thread_dispatch
 
 	/* We are done with thread dispatching */
 	li	SCRATCH_0_REGISTER, 0
-	stw	SCRATCH_0_REGISTER, PER_CPU_ISR_DISPATCH_DISABLE(SELF_CPU_REGISTER)
+	stw	SCRATCH_0_REGISTER, PER_CPU_ISR_DISPATCH_DISABLE(SCRATCH_1_REGISTER)
 
 .Lthread_dispatch_done:
 
@@ -424,8 +381,8 @@ ppc_exc_interrupt:
 
 #ifdef __SPE__
 	/* Load SPEFSCR and ACC */
-	lwz	DISPATCH_LEVEL_REGISTER, PPC_EXC_SPEFSCR_OFFSET(r1)
-	evldd	HANDLER_REGISTER, PPC_EXC_ACC_OFFSET(r1)
+	lwz	SCRATCH_3_REGISTER, PPC_EXC_SPEFSCR_OFFSET(r1)
+	evldd	SCRATCH_4_REGISTER, PPC_EXC_ACC_OFFSET(r1)
 #endif
 
 	/*
@@ -454,45 +411,44 @@ ppc_exc_interrupt:
 	stwcx.	SCRATCH_0_REGISTER, r1, SCRATCH_0_REGISTER
 
 	/* Load SRR0, SRR1, CR, XER, CTR, and LR */
-	PPC_REG_LOAD	SCRATCH_0_REGISTER, SRR0_FRAME_OFFSET(r1)
-	PPC_REG_LOAD	SCRATCH_1_REGISTER, SRR1_FRAME_OFFSET(r1)
-	lwz	SCRATCH_2_REGISTER, EXC_CR_OFFSET(r1)
-	lwz	SCRATCH_3_REGISTER, EXC_XER_OFFSET(r1)
-	PPC_REG_LOAD	SCRATCH_4_REGISTER, EXC_CTR_OFFSET(r1)
-	PPC_REG_LOAD	SCRATCH_5_REGISTER, EXC_LR_OFFSET(r1)
-
-	PPC_GPR_LOAD	VECTOR_REGISTER, VECTOR_OFFSET(r1)
-	PPC_GPR_LOAD	SELF_CPU_REGISTER, SELF_CPU_OFFSET(r1)
-	PPC_GPR_LOAD	ISR_NEST_REGISTER, ISR_NEST_OFFSET(r1)
-
-#ifdef __SPE__
-	/* Restore SPEFSCR */
-	mtspr	FSL_EIS_SPEFSCR, DISPATCH_LEVEL_REGISTER
-#endif
-	PPC_GPR_LOAD	DISPATCH_LEVEL_REGISTER, DISPATCH_LEVEL_OFFSET(r1)
-
-#ifdef __SPE__
-	/* Restore ACC */
-	evmra	HANDLER_REGISTER, HANDLER_REGISTER
-#endif
-	PPC_GPR_LOAD	HANDLER_REGISTER, HANDLER_OFFSET(r1)
-
-	/* Restore SRR0, SRR1, CR, CTR, XER, and LR */
-	mtsrr0	SCRATCH_0_REGISTER
+	PPC_REG_LOAD	SCRATCH_5_REGISTER, SRR0_FRAME_OFFSET(r1)
+	PPC_REG_LOAD	SCRATCH_6_REGISTER, SRR1_FRAME_OFFSET(r1)
+	lwz	SCRATCH_7_REGISTER, EXC_CR_OFFSET(r1)
+	lwz	SCRATCH_8_REGISTER, EXC_XER_OFFSET(r1)
+	PPC_REG_LOAD	SCRATCH_9_REGISTER, EXC_CTR_OFFSET(r1)
+	PPC_REG_LOAD	SCRATCH_10_REGISTER, EXC_LR_OFFSET(r1)
+
+	/* Restore volatile registers */
 	PPC_GPR_LOAD	SCRATCH_0_REGISTER, SCRATCH_0_OFFSET(r1)
-	mtsrr1	SCRATCH_1_REGISTER
 #ifdef __powerpc64__
 	PPC_GPR_LOAD	r2, GPR2_OFFSET(r1)
 #endif
 	PPC_GPR_LOAD	SCRATCH_1_REGISTER, SCRATCH_1_OFFSET(r1)
-	mtcr	SCRATCH_2_REGISTER
 	PPC_GPR_LOAD	SCRATCH_2_REGISTER, SCRATCH_2_OFFSET(r1)
-	mtxer	SCRATCH_3_REGISTER
+
+#ifdef __SPE__
+	/* Restore SPEFSCR and ACC */
+	mtspr	FSL_EIS_SPEFSCR, SCRATCH_3_REGISTER
+	evmra	SCRATCH_4_REGISTER, SCRATCH_4_REGISTER
+#endif
+
+	/* Restore volatile registers */
 	PPC_GPR_LOAD	SCRATCH_3_REGISTER, SCRATCH_3_OFFSET(r1)
-	mtctr	SCRATCH_4_REGISTER
 	PPC_GPR_LOAD	SCRATCH_4_REGISTER, SCRATCH_4_OFFSET(r1)
-	mtlr	SCRATCH_5_REGISTER
+
+	/* Restore SRR0, SRR1, CR, CTR, XER, and LR plus volatile registers */
+	mtsrr0	SCRATCH_5_REGISTER
 	PPC_GPR_LOAD	SCRATCH_5_REGISTER, SCRATCH_5_OFFSET(r1)
+	mtsrr1	SCRATCH_6_REGISTER
+	PPC_GPR_LOAD	SCRATCH_6_REGISTER, SCRATCH_6_OFFSET(r1)
+	mtcr	SCRATCH_7_REGISTER
+	PPC_GPR_LOAD	SCRATCH_7_REGISTER, SCRATCH_7_OFFSET(r1)
+	mtxer	SCRATCH_8_REGISTER
+	PPC_GPR_LOAD	SCRATCH_8_REGISTER, SCRATCH_8_OFFSET(r1)
+	mtctr	SCRATCH_9_REGISTER
+	PPC_GPR_LOAD	SCRATCH_9_REGISTER, SCRATCH_9_OFFSET(r1)
+	mtlr	SCRATCH_10_REGISTER
+	PPC_GPR_LOAD	SCRATCH_10_REGISTER, SCRATCH_10_OFFSET(r1)
 
 	/* Pop stack */
 	addi	r1, r1, PPC_EXC_INTERRUPT_FRAME_SIZE
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/vectors.h b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/vectors.h
index 56c9e64991..81526eb4b0 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/vectors.h
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/vectors.h
@@ -422,7 +422,7 @@ extern uint32_t ppc_exc_cache_wb_check;
   /**
    * @brief Interrupt dispatch routine provided by BSP.
    */
-  void bsp_interrupt_dispatch(void);
+  void bsp_interrupt_dispatch(uintptr_t exception_number);
 #endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
 
 /**