adapted powerpc exception code

1 files changed, 581 insertions, 333 deletions

diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
index 4f66d91e54..106e73bd6d 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
@@ -3,6 +3,8 @@
  *
  * Modified and partially rewritten by Till Straumann, 2007-2008
  *
+ * Modified by Sebastian Huber <sebastian.huber@embedded-brains.de>, 2008.
+ *
  * Low-level assembly code for PPC exceptions (macros).
  *
  * This file was written with the goal to eliminate
@@ -10,116 +12,33 @@
  * reintroduce such statements.
  */
 
-#include <rtems/score/cpu.h>
-#include <bsp/vectors.h>
+#include <libcpu/powerpc-utility.h>
 #include <libcpu/raw_exception.h>
 
-#define EXC_MIN_GPR1		0
-#define FRAME_LINK_SPACE	8
-
-
-#define r0	0
-#define r1	1
-#define r2	2
-#define r3	3
-#define r4	4
-#define r5	5
-#define r6	6
-#define r7	7
-#define r8	8
-#define r9	9
-#define r10	10
-#define r11	11
-#define r12	12
-#define r13	13
-#define r14	14
-#define r15	15
-#define r16	16
-#define r17	17
-#define r18	18
-#define r19	19
-#define r20	20
-#define r21	21
-#define r22	22
-#define r23	23
-#define r24	24
-#define r25	25
-#define r26	26
-#define r27	27
-#define r28	28
-#define r29	29
-#define r30	30
-#define r31	31
-
-#define cr0 0
-#define cr1 1
-#define cr4 4
-
-#define LT(cr)	((cr)*4+0)
-#define GT(cr)	((cr)*4+1)
-#define EQ(cr)	((cr)*4+2)
-
-#define	NOFRAME	0xffff8000
-
-/* Opcode of  'stw r1, off(r13)' */
-#define STW_R1_R13(off)	((((36<<10)|(r1<<5)|(r13))<<16) | ((off)&0xffff))
+#include "vectors.h"
 
-/*
- **********************************************************************
- * MACRO: SWITCH_STACK
- **********************************************************************
- *
- * Increment _ISR_Nest_level and switch r1 to interrupt
- * stack if not already there.
- *
- * USES:    RA, RB, cr0
- * ON EXIT: RA, RB available, r1 points into interrupt
- *          stack.
- *
- * NOTES:
- *   - NEVER store stuff in a frame before
- *     reserving it (stwu r1) - otherwise
- *     higher-priority exception may overwrite.
- *   - algorithm should allow nesting of higher
- *     priority exceptions (HPE) (by disabling
- *     them while the stack is switched).
- */
-#if 1
-	.macro	SWITCH_STACK RA RB FLVR
-	mfspr	\RB, SPRG1
-	cmplw	cr0, r1, \RB
-	bgt		do_r1_reload_\FLVR
-	lwz     \RA, ppc_exc_intr_stack_size@sdarel(r13)
-	subf	\RB, \RA, \RB
-	cmplw	cr0, r1, \RB
-	bge		no_r1_reload_\FLVR
-do_r1_reload_\FLVR:
-	mfspr	r1, SPRG1
-no_r1_reload_\FLVR:
-	lwz		\RA, _ISR_Nest_level@sdarel(r13)
-	addi	\RA, \RA, 1
-	stw		\RA, _ISR_Nest_level@sdarel(r13)
-	.endm
-#else
-	.macro	SWITCH_STACK RA RB FLVR
-	/* disable interrupts */
-	lwz		\RA, ppc_exc_msr_irq_mask@sdarel(r13)
-	mfmsr	\RB
-	andc	\RA, \RB, \RA
-	mtmsr	\RA
-	/* increment nest level */
-	lwz		\RA, _ISR_Nest_level@sdarel(r13)
-	cmplwi	cr0, \RA, 0
-	bne	no_r1_reload_\FLVR
-	/* reload r1            */
-	mfspr	r1, SPRG1
-no_r1_reload_\FLVR:
-	addi	\RA, \RA, 1
-	stw		\RA, _ISR_Nest_level@sdarel(r13)
-	/* restore IRQ mask     */
-	mtmsr	\RB
-	.endm
-#endif
+#define LT(cr) ((cr)*4+0)
+#define GT(cr) ((cr)*4+1)
+#define EQ(cr) ((cr)*4+2)
+
+/* Opcode of 'stw r1, off(r13)' */
+#define STW_R1_R13(off) ((((36<<10)|(r1<<5)|(r13))<<16) | ((off)&0xffff))
+
+#define FRAME_REGISTER r14
+#define VECTOR_REGISTER r4
+#define SCRATCH_REGISTER_0 r5
+#define SCRATCH_REGISTER_1 r6
+#define SCRATCH_REGISTER_2 r7
+
+#define FRAME_OFFSET( r) GPR14_OFFSET( r)
+#define VECTOR_OFFSET( r) GPR4_OFFSET( r)
+#define SCRATCH_REGISTER_0_OFFSET( r) GPR5_OFFSET( r)
+#define SCRATCH_REGISTER_1_OFFSET( r) GPR6_OFFSET( r)
+#define SCRATCH_REGISTER_2_OFFSET( r) GPR7_OFFSET( r)
+
+#define CR_TYPE 2
+#define CR_MSR 3
+#define CR_LOCK 4
 
 	/*
 	 * Minimal prologue snippets:
@@ -146,25 +65,27 @@ no_r1_reload_\FLVR:
 	 */ 
 
 /*
- **********************************************************************
+ *****************************************************************************
  * MACRO: PPC_EXC_MIN_PROLOG_ASYNC
- **********************************************************************
- * USES:    r3
- * ON EXIT: vector in r3
+ *****************************************************************************
+ * USES:    VECTOR_REGISTER
+ * ON EXIT: Vector in VECTOR_REGISTER
  *
- * NOTES:   r3 saved in special variable 'ppc_exc_gpr3_\_PRI'
+ * NOTES:   VECTOR_REGISTER saved in special variable
+ *          'ppc_exc_vector_register_\_PRI'.
  *
  */
 	.macro	PPC_EXC_MIN_PROLOG_ASYNC _NAME _VEC _PRI _FLVR
+
 	.global	ppc_exc_min_prolog_async_\_NAME
 ppc_exc_min_prolog_async_\_NAME:
-	/* Atomically write lock variable in 1st instruction with non-zero value
-	 * (r1 is always nonzero; r13 could also be used)
+	/* Atomically write lock variable in 1st instruction with non-zero
+	 * value (r1 is always nonzero; r13 could also be used)
 	 *
 	 * NOTE: raising an exception and executing this first instruction
-	 *       of the exception handler is apparently NOT atomic, i.e.,
-	 *       a low-priority IRQ could set the PC to this location and
-	 *       a critical IRQ could intervene just at this point.
+	 *       of the exception handler is apparently NOT atomic, i.e., a
+	 *       low-priority IRQ could set the PC to this location and a
+	 *       critical IRQ could intervene just at this point.
 	 *
 	 *       We check against this pathological case by checking the
 	 *       opcode/instruction at the interrupted PC for matching
@@ -178,322 +99,649 @@ ppc_exc_min_prolog_async_\_NAME:
 	 *          2) This instruction (including the address offset)
 	 *             is not used anywhere else (probably a safe assumption).
 	 */
-	stw		r1, ppc_exc_lock_\_PRI@sdarel(r13)
-	/* We have no stack frame yet; store r3 in special area;
+	stw	r1, ppc_exc_lock_\_PRI@sdarel(r13)
+	/*	We have no stack frame yet; store VECTOR_REGISTER in special area;
 	 * a higher-priority (critical) interrupt uses a different area
 	 * (hence the different prologue snippets) (\PRI) 
 	 */
-	stw		r3, ppc_exc_gpr3_\_PRI@sdarel(r13)
-	/* Load vector.
+	stw	VECTOR_REGISTER, ppc_exc_vector_register_\_PRI@sdarel(r13)
+	/*	Load vector.
 	 */
-	li		r3, ( \_VEC | 0xffff8000 )
-	/* Branch (must be within 32MB)
+	li	VECTOR_REGISTER, ( \_VEC | 0xffff8000 )
+	/*	Branch (must be within 32MB)
 	 */
-	ba 		wrap_\_FLVR
+	ba	wrap_\_FLVR
+
 	.endm
 
 /*
- **********************************************************************
+ *****************************************************************************
  * MACRO: PPC_EXC_MIN_PROLOG_SYNC
- **********************************************************************
- * USES:    r3
- * ON EXIT: vector in r3
+ *****************************************************************************
+ * USES:    VECTOR_REGISTER
+ * ON EXIT: vector in VECTOR_REGISTER
  *
- * NOTES:   exception stack frame pushed; r3 saved in frame
+ * NOTES:   exception stack frame pushed; VECTOR_REGISTER saved in frame
  *
  */
-	.macro PPC_EXC_MIN_PROLOG_SYNC _NAME _VEC _PRI _FLVR
-	.global ppc_exc_min_prolog_sync_\_NAME
+	.macro	PPC_EXC_MIN_PROLOG_SYNC _NAME _VEC _PRI _FLVR
+
+	.global	ppc_exc_min_prolog_sync_\_NAME
 ppc_exc_min_prolog_sync_\_NAME:
 	stwu	r1, -EXCEPTION_FRAME_END(r1)
-	stw		r3, GPR3_OFFSET(r1)
-	li		r3, \_VEC
-	ba		wrap_nopush_\_FLVR
+	stw	VECTOR_REGISTER, VECTOR_OFFSET(r1)
+	li	VECTOR_REGISTER, \_VEC
+	ba	wrap_nopush_\_FLVR
+
 	.endm
 		
 /*
- **********************************************************************
+ *****************************************************************************
  * MACRO: TEST_1ST_OPCODE_crit
- **********************************************************************
+ *****************************************************************************
  *
- * USES:    REG, cr4
- * ON EXIT: REG available (contains *pc - STW_R1_R13(0)), return value in cr4
+ * USES:    REG, CR_LOCK
+ * ON EXIT: REG available (contains *pc - STW_R1_R13(0)),
+ *          return value in CR_LOCK.
  *
- * test opcode interrupted by critical (asynchronous) exception;
- * set cr4 if
+ * test opcode interrupted by critical (asynchronous) exception; set CR_LOCK if
  *
- *   *SRR0 == 'stw r1, ppc_exc_std_lock@sdarel(r13)'
+ *   *SRR0 == 'stw r1, ppc_exc_lock_std@sdarel(r13)'
  *
  */
-	.macro TEST_1ST_OPCODE_crit _REG _SRR0
-	mf\_SRR0 \_REG
-	lwz		\_REG, 0(\_REG)
-	/* opcode now in REG */
+	.macro	TEST_1ST_OPCODE_crit _REG
+
+	lwz	\_REG, SRR0_FRAME_OFFSET(FRAME_REGISTER)
+	lwz	\_REG, 0(\_REG)
+	/*	opcode now in REG */
 
-	/* subtract upper 16bits of 'stw r1, 0(r13)' instruction */
+	/*	subtract upper 16bits of 'stw r1, 0(r13)' instruction */
 	subis	\_REG, \_REG, STW_R1_R13(0)@h
 	/*
 	 * if what's left compares against the 'ppc_exc_lock_std@sdarel'
 	 * address offset then we have a match...
 	 */
-	cmpli   cr4, \_REG, ppc_exc_lock_std@sdarel
-	.endm
+	cmpli	CR_LOCK, \_REG, ppc_exc_lock_std@sdarel
 
-/*
- **********************************************************************
- * MACRO: TEST_1ST_OPCODE_mchk
- **********************************************************************
- * USES:    REG, cr0, cr4
- * ON EXIT: REG, cr0 available, return value in cr4
- *
- * test opcode interrupted by (asynchronous) machine-check exception;
- * set cr4 if
- *
- *   *SRR0 == 'stw r1, ppc_exc_std_lock@sdarel(r13)'
- *
- * OR
- *
- *   *SRR0 == 'stw r1, ppc_exc_crit_lock@sdarel(r13)'
- *
- */
-	.macro TEST_1ST_OPCODE_mchk _REG _SRR0
-	TEST_1ST_OPCODE_crit _REG=\_REG _SRR0=\_SRR0
-	cmpli    cr0, \_REG, ppc_exc_lock_crit@sdarel
-	/* cr4 set if 1st opcode matches writing either lock */
-	cror     EQ(cr4), EQ(cr4), EQ(cr0)
 	.endm
 
 /*
- **********************************************************************
+ *****************************************************************************
  * MACRO: TEST_LOCK_std
- **********************************************************************
+ *****************************************************************************
  *
- * USES:    cr4
- * ON EXIT: cr4 is set (indicates no lower-priority locks are engaged)
+ * USES:    CR_LOCK
+ * ON EXIT: CR_LOCK is set (indicates no lower-priority locks are engaged)
  *
  */
-	.macro TEST_LOCK_std _SRR0 _FLVR
-	/* 'std' is lowest level, i.e., can not be locked -> EQ(cr4) = 1 */
-	creqv EQ(cr4), EQ(cr4), EQ(cr4)
+	.macro	TEST_LOCK_std _FLVR
+	/* 'std' is lowest level, i.e., can not be locked -> EQ(CR_LOCK) = 1 */
+	creqv	EQ(CR_LOCK), EQ(CR_LOCK), EQ(CR_LOCK)
 	.endm
 
 /*
- **********************************************************************
+ ******************************************************************************
  * MACRO: TEST_LOCK_crit
- **********************************************************************
+ ******************************************************************************
  *
- * USES:    cr4, cr0, r4, r5
- * ON EXIT: cr0, r4, r5 available, returns result in cr4
+ * USES:    CR_LOCK, cr0, SCRATCH_REGISTER_0, SCRATCH_REGISTER_1
+ * ON EXIT: cr0, SCRATCH_REGISTER_0, SCRATCH_REGISTER_1 available,
+ *          returns result in CR_LOCK.
  *
  * critical-exception wrapper has to check 'std' lock:
  *
- * Return cr4 = (   ppc_std_lock == 0
- *               && * _SRR0 != <write std lock instruction>
- *               && ppc_exc_crit_always_enabled == 0 )
+ * Return CR_LOCK = (   ppc_lock_std == 0
+ *               && * SRR0 != <write std lock instruction> )
  *
  */
-	.macro TEST_LOCK_crit _SRR0 _FLVR
-	/* Are critical exceptions always enabled ? */
-	lwz	r4, ppc_exc_crit_always_enabled@sdarel(r13)
-	cmpwi cr4, r4, 0
-	bne  cr4, TEST_LOCK_crit_done_\_FLVR
-
-	/* STD interrupt could have been interrupted before
-	 * executing the 1st instruction which sets the lock;
-	 * check this case by looking at the opcode present
-	 * at the interrupted PC location.
+	.macro	TEST_LOCK_crit _FLVR
+
+	/* STD interrupt could have been interrupted before executing the 1st
+	 * instruction which sets the lock; check this case by looking at the
+	 * opcode present at the interrupted PC location.
 	 */
-	TEST_1ST_OPCODE_crit _REG=r4 _SRR0=\_SRR0
+	TEST_1ST_OPCODE_crit	_REG=SCRATCH_REGISTER_0
 	/*
-	 * At this point cr4 is set if
+	 * At this point CR_LOCK is set if
 	 *
 	 *   *(PC) == 'stw r1, ppc_exc_lock_std@sdarel(r13)'
 	 *
 	 */
 	
 	/* check lock */
-	lwz r5, ppc_exc_lock_std@sdarel(r13)
-	cmpli	cr0, r5, 0
+	lwz	SCRATCH_REGISTER_1, ppc_exc_lock_std@sdarel(r13)
+	cmpli	cr0, SCRATCH_REGISTER_1, 0
 	/*
 	 *
-	 * cr4 = (   *pc != <write std lock instruction>
+	 * CR_LOCK = (   *pc != <write std lock instruction>
 	 *        && ppc_exc_lock_std == 0 )
 	 */
-	crandc  EQ(cr4), EQ(cr0), EQ(cr4)
-TEST_LOCK_crit_done_\_FLVR:
+	crandc	EQ(CR_LOCK), EQ(cr0), EQ(CR_LOCK)
+
 	.endm
 
-#if 0
 /*
- **********************************************************************
+ ******************************************************************************
  * MACRO: TEST_LOCK_mchk
- **********************************************************************
- *
- * USES:    cr4, cr0, r4, r5
- * ON EXIT: cr0, r4, r5 available, returns result in cr4
- *
- * machine-check wrapper has to check 'std' and 'crit' locks, i.e.,
- *
- * Return cr4 = (   * _SRR0 != <write std  lock instruction>
- *               && * _SRR0 != <write crit lock instruction> )
- *               && ppc_std_lock  == 0
- *               && ppc_crit_lock == 0 )
+ ******************************************************************************
+ *
+ * USES:    CR_LOCK
+ * ON EXIT: CR_LOCK is cleared.
+ *
+ * We never want to disable machine-check exceptions to avoid a checkstop. This
+ * means that we cannot use enabling/disabling this type of exception for
+ * protection of critical OS data structures.  Therefore, calling OS primitives
+ * from a machine-check handler is ILLEGAL. Since machine-checks can happen
+ * anytime it is not legal to perform a context switch (since the exception
+ * could hit a IRQ protected section of code).  We simply let this test return
+ * 0 so that ppc_exc_wrapup is never called after handling a machine-check.
  */
-	.macro TEST_LOCK_mchk _SRR0 _FLVR
-	TEST_1ST_OPCODE_mchk _REG=r4 _SRR0=\_SRR0
-	/* cr4 set if 1st opcode matches writing either lock */
-
-	/* proceed checking the locks */
-	lwz r5, ppc_exc_lock_std@sdarel(r13)
-	lwz r4, ppc_exc_lock_crit@sdarel(r13)
-	/* set cr0 if neither lock is set */
-	or.     r4, r4, r5
-	/* set cr4 if
-	 *     cr0 is set   (neither lock set)
-	 * AND cr4 is clear (interrupted opcode doesn't match writing any lock)
-	 */
-	crandc	EQ(cr4), EQ(cr0), EQ(cr4)
+	.macro	TEST_LOCK_mchk _SRR0 _FLVR
+
+	crxor	EQ(CR_LOCK), EQ(CR_LOCK), EQ(CR_LOCK)
+
 	.endm
-#else
+
 /*
- **********************************************************************
- * MACRO: TEST_LOCK_mchk
- **********************************************************************
+ ******************************************************************************
+ * MACRO: RECOVER_CHECK_\PRI
+ ******************************************************************************
  *
- * USES:    cr4
- * ON EXIT: cr4 is cleared.
+ * USES:    cr0, SCRATCH_REGISTER_0, SCRATCH_REGISTER_1
+ * ON EXIT: cr0, SCRATCH_REGISTER_0, SCRATCH_REGISTER_1 available
  *
- * We never want to disable machine-check exceptions to avoid
- * a checkstop. This means that we cannot use enabling/disabling
- * this type of exception for protection of critical OS data structures.
- * Therefore, calling OS primitives from a machine-check handler
- * is ILLEGAL. Since machine-checks can happen anytime it is not
- * legal to perform a context switch (since the exception could
- * hit a IRQ protected section of code).
- * We simply let this test return 0 so that ppc_exc_wrapup is
- * never called after handling a machine-check.
+ * Checks if the exception is recoverable for exceptions which need such a
+ * test.
  */
-	.macro TEST_LOCK_mchk _SRR0 _FLVR
-	crxor	EQ(cr4), EQ(cr4), EQ(cr4)
+
+/* Standard*/
+	.macro	RECOVER_CHECK_std _FLVR
+
+	/* Check if exception is recoverable */
+	lwz	SCRATCH_REGISTER_0, SRR1_FRAME_OFFSET(FRAME_REGISTER)
+	lwz	SCRATCH_REGISTER_1, ppc_exc_msr_bits@sdarel(r13)
+	xor	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, SCRATCH_REGISTER_0
+	andi.	SCRATCH_REGISTER_0, SCRATCH_REGISTER_1, MSR_RI
+
+recover_check_twiddle_std_\_FLVR:
+
+	/* Not recoverable? */
+	bne	recover_check_twiddle_std_\_FLVR
+
 	.endm
-#endif
 
+/* Critical */
+	.macro	RECOVER_CHECK_crit _FLVR
+
+	/* Nothing to do */
+
+	.endm
+
+/* Machine check */
+	.macro	RECOVER_CHECK_mchk _FLVR
+
+	/* Check if exception is recoverable */
+	lwz	SCRATCH_REGISTER_0, SRR1_FRAME_OFFSET(FRAME_REGISTER)
+	lwz	SCRATCH_REGISTER_1, ppc_exc_msr_bits@sdarel(r13)
+	xor	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, SCRATCH_REGISTER_0
+	andi.	SCRATCH_REGISTER_0, SCRATCH_REGISTER_1, MSR_RI
+
+recover_check_twiddle_mchk_\_FLVR:
+
+	/* Not recoverable? */
+	bne	recover_check_twiddle_mchk_\_FLVR
+
+	.endm
 
 /*
- **********************************************************************
+ ******************************************************************************
  * MACRO: WRAP
- **********************************************************************
- *
- * Minimal prologue snippets jump into WRAP
- * which prepares calling code common to all
- * flavors of exceptions.
- * We must have this macro instantiated for
- * each possible flavor of exception so that
- * we use the proper lock variable, SRR register pair and
- * RFI instruction.
- *
+ ******************************************************************************
+ *
+ * Minimal prologue snippets jump into WRAP which calls the high level
+ * exception handler.  We must have this macro	instantiated for each possible
+ * flavor of exception so that we use the proper lock variable, SRR register
+ * pair and RFI instruction.
+ *
+ * We have two types of exceptions: synchronous and asynchronous (= interrupt
+ * like).  The type is encoded in the vector register (= VECTOR_REGISTER).  For
+ * interrupt like exceptions the MSB in the vector register is set.  The
+ * exception type is kept in the comparison register CR_TYPE.  Normal
+ * exceptions (MSB is clear) use the task stack and a context switch may happen
+ * at any time.  The interrupt like exceptions disable thread dispatching and
+ * switch to the interrupt stack (base address is in SPRG1).
+ *
+ *                                      +
+ *                                      |
+ *                                      | Minimal prologue
+ *                                      |
+ *                                      +
+ *                                      |
+ *                                      | o Setup frame pointer
+ *                                      | o Save basic registers
+ *                                      | o Determine exception type:
+ *                                      |   synchronous or asynchronous
+ *                                      |
+ *                                +-----+
+ * Synchronous exceptions:        |     | Asynchronous exceptions:
+ *                                |     |
+ * Save non-volatile registers    |     | o Increment thread dispatch
+ *                                |     |   disable level
+ *                                |     | o Increment ISR nest level
+ *                                |     | o Clear lock
+ *                                |     | o Switch stack if necessary
+ *                                |     |
+ *                                +---->+
+ *                                      |
+ *                                      | o Save volatile registers
+ *                                      | o Change MSR if necessary
+ *                                      | o Call high level handler
+ *                                      | o Call global handler if necessary
+ *                                      | o Check if exception is recoverable
+ *                                      |
+ *                                +-----+
+ * Synchronous exceptions:        |     | Asynchronous exceptions:
+ *                                |     |
+ * Restore non-volatile registers |     | o Decrement ISR nest level
+ *                                |     | o Switch stack
+ *                                |     | o Decrement thread dispatch
+ *                                |     |   disable level
+ *                                |     | o Test lock
+ *                                |     | o May do a context switch
+ *                                |     |
+ *                                +---->+
+ *                                      |
+ *                                      | o Restore MSR if necessary
+ *                                      | o Restore volatile registers
+ *                                      | o Restore frame pointer
+ *                                      | o Return
+ *                                      |
+ *                                      +
  */
 	.macro	WRAP _FLVR _PRI _SRR0 _SRR1 _RFI
+
 wrap_\_FLVR:
-	stwu    r1,  -EXCEPTION_FRAME_END(r1)
+
+	/* Push exception frame */
+	stwu	r1, -EXCEPTION_FRAME_END(r1)
+
 wrap_nopush_\_FLVR:
-	stw		r14, GPR14_OFFSET(r1)
-wrap_no_save_r14_\_FLVR:
-
-	/* Save r4 r5 and CR; we want CR soon */
-	mfcr	r14
-	stw		r4,  GPR4_OFFSET(r1)
-	stw		r5,  GPR5_OFFSET(r1)
-	stw		r14, EXC_CR_OFFSET(r1)
-
-	/* Check if this is an 'interrupt-type' exception
-	 * (MSB vector is set).
-	 * 'interrupt-type' exceptions disable thread dispatching
-	 * and switch to a private stack.
-	 * The type of exception is kept in (non-volatile) cr2
-	 *  < 0 -> interrupt-type
-	 *  > 0 -> 'normal' exception; always on task stack,
-	 *         may switch context at any time.
+
+	/* Save frame register */
+	stw	FRAME_REGISTER, FRAME_OFFSET(r1)
+
+wrap_no_save_frame_register_\_FLVR:
+
+	/*
+	 * We save at first only some scratch registers
+	 * and the CR.  We use a non-volatile register
+	 * for the exception frame pointer (= FRAME_REGISTER).
+	 */
+
+	/* Move frame address in non-volatile FRAME_REGISTER */
+	mr	FRAME_REGISTER, r1
+
+	/* Save scratch registers */
+	stw	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0_OFFSET(FRAME_REGISTER)
+	stw	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1_OFFSET(FRAME_REGISTER)
+	stw	SCRATCH_REGISTER_2, SCRATCH_REGISTER_2_OFFSET(FRAME_REGISTER)
+
+	/* Save CR */
+	mfcr	SCRATCH_REGISTER_0
+	stw	SCRATCH_REGISTER_0, EXC_CR_OFFSET(FRAME_REGISTER)
+
+	/* Check exception type and remember it in non-volatile CR_TYPE */
+	cmpwi	CR_TYPE, VECTOR_REGISTER, 0
+
+	/*
+	 * Depending on the exception type we do now save the non-volatile
+	 * registers or disable thread dispatching and switch to the ISR stack.
+	 */
+
+	/* Branch for synchronous exceptions */
+	bge	CR_TYPE, wrap_save_non_volatile_regs_\_FLVR
+
+	/*
+	 * Increment the thread dispatch disable level in case a higher
+	 * priority exception occurs we don't want it to run the scheduler.  It
+	 * is safe to increment this without disabling higher priority
+	 * exceptions since those will see that we wrote the lock anyways.
 	 */
-	cmpwi	cr2, r3, 0
 
-	/* 
-	 * Save frame address in r14
+	/* Increment ISR nest level and thread dispatch disable level */
+	lwz	SCRATCH_REGISTER_0, _ISR_Nest_level@sdarel(r13)
+	lwz	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+	addi	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0, 1
+	addi	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, 1
+	stw	SCRATCH_REGISTER_0, _ISR_Nest_level@sdarel(r13)
+	stw	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+
+	/*
+	 * No higher-priority exception occurring after this point
+	 * can cause a context switch.
 	 */
-	mr      r14, r1
 
-	bge     cr2, no_thread_dispatch_disable_\_FLVR
+	/* Clear lock */
+	li	SCRATCH_REGISTER_0, 0
+	stw	SCRATCH_REGISTER_0, ppc_exc_lock_\_PRI@sdarel(r13)
+
+	/* Switch stack if necessary */
+	mfspr	SCRATCH_REGISTER_0, SPRG1
+	cmpw	SCRATCH_REGISTER_0, r1
+	blt	wrap_stack_switch_\_FLVR
+	mfspr	SCRATCH_REGISTER_1, SPRG2
+	cmpw	SCRATCH_REGISTER_1, r1
+	blt	wrap_stack_switch_done_\_FLVR
+
+wrap_stack_switch_\_FLVR:
+
+	mr	r1, SCRATCH_REGISTER_0
 
-	/* first thing we need to
-	 * increment the thread-dispatch disable level
-	 * in case a higher priority exception occurs
-	 * we don't want it to run the scheduler.
-	 * (It is safe to increment this w/o disabling
-	 * higher priority interrupts since those will
-	 * see that we wrote the lock anyways).
+wrap_stack_switch_done_\_FLVR:
+
+	/*
+	 * Load the pristine VECTOR_REGISTER from a special location for
+	 * asynchronous exceptions.  The synchronous exceptions save the
+	 * VECTOR_REGISTER in their minimal prologue.
 	 */
-	lwz		r5,  _Thread_Dispatch_disable_level@sdarel(r13)
-	addi	r5,  r5, 1
-	stw		r5,  _Thread_Dispatch_disable_level@sdarel(r13)
+	lwz	SCRATCH_REGISTER_2, ppc_exc_vector_register_\_PRI@sdarel(r13)
+
+	/* Save pristine vector register */
+	stw	SCRATCH_REGISTER_2, VECTOR_OFFSET(FRAME_REGISTER)
 
-	/* clear lock; no higher-priority interrupt occurring after
-	 * this point can cause a context switch.
+wrap_disable_thread_dispatching_done_\_FLVR:
+
+	/*
+	 * We now have SCRATCH_REGISTER_0, SCRATCH_REGISTER_1,
+	 * SCRATCH_REGISTER_2 and CR available.  VECTOR_REGISTER still holds
+	 * the vector (and exception type).  FRAME_REGISTER is a pointer to the
+	 * exception frame (always on the stack of the interrupted context).
+	 * r1 is the stack pointer, either on the task stack or on the ISR
+	 * stack.  CR_TYPE holds the exception type.
 	 */
-	li		r5,  0
-	stw		r5,  ppc_exc_lock_\_PRI@sdarel(r13)
 
-	/* test lower-priority locks; result in (non-volatile) cr4 */
-	TEST_LOCK_\_PRI _SRR0=\_SRR0 _FLVR=\_FLVR
+	/* Save SRR0 */
+	mfspr	SCRATCH_REGISTER_0, \_SRR0
+	stw	SCRATCH_REGISTER_0, SRR0_FRAME_OFFSET(FRAME_REGISTER)
 
-	/* Peform stack switch if necessary */
-	SWITCH_STACK RA=r4 RB=r5 FLVR=\_FLVR
+	/* Save SRR1 */
+	mfspr	SCRATCH_REGISTER_0, \_SRR1
+	stw	SCRATCH_REGISTER_0, SRR1_FRAME_OFFSET(FRAME_REGISTER)
 
-	/* save r3, in exception frame */
-	lwz		r5,  ppc_exc_gpr3_\_PRI@sdarel(r13)
-	stw		r5,  GPR3_OFFSET(r14)
+	/* Save CTR */
+	mfctr	SCRATCH_REGISTER_0
+	stw	SCRATCH_REGISTER_0, EXC_CTR_OFFSET(FRAME_REGISTER)
 
-no_thread_dispatch_disable_\_FLVR:
+	/* Save XER */
+	mfxer	SCRATCH_REGISTER_0
+	stw	SCRATCH_REGISTER_0, EXC_XER_OFFSET(FRAME_REGISTER)
 
-	/* save lr into exception frame */
-	mflr	r4
-	stw     r4,  EXC_LR_OFFSET(r14)
+	/* Save LR */
+	mflr	SCRATCH_REGISTER_0
+	stw	SCRATCH_REGISTER_0, EXC_LR_OFFSET(FRAME_REGISTER)
 
-	/* we now have r4,r5,lr,cr available;
-	 * r3  still holds the vector,
-	 * r14 a pointer to the exception frame (always on
-	 *     task stack)
-	 * r1  is the stack pointer, either on the task stack
-	 *     or on the IRQ stack
+	/* Save volatile registers */
+	stw	r0, GPR0_OFFSET(FRAME_REGISTER)
+	stw	r3, GPR3_OFFSET(FRAME_REGISTER)
+	stw	r8, GPR8_OFFSET(FRAME_REGISTER)
+	stw	r9, GPR9_OFFSET(FRAME_REGISTER)
+	stw	r10, GPR10_OFFSET(FRAME_REGISTER)
+	stw	r11, GPR11_OFFSET(FRAME_REGISTER)
+	stw	r12, GPR12_OFFSET(FRAME_REGISTER)
+
+	/* Save read-only small data area anchor (EABI) */
+	stw	r2, GPR2_OFFSET(FRAME_REGISTER)
+
+	/* Save vector number and exception type */
+	stw	VECTOR_REGISTER, EXCEPTION_NUMBER_OFFSET(FRAME_REGISTER)
+
+	/* Load MSR bit mask */
+	lwz	SCRATCH_REGISTER_0, ppc_exc_msr_bits@sdarel(r13)
+
+	/*
+	 * Change the MSR if necessary (MMU, RI),
+	 * remember decision in non-volatile CR_MSR
+	 */
+	cmpwi	CR_MSR, SCRATCH_REGISTER_0, 0
+	bne	CR_MSR, wrap_change_msr_\_FLVR
+
+wrap_change_msr_done_\_FLVR:
+
+	/*
+	 * Call high level exception handler
+	 */
+
+	/*
+	 * 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_REGISTER_1, VECTOR_REGISTER, 2, 25, 29
+
+	/* Load handler table address */
+	LA	SCRATCH_REGISTER_0, ppc_exc_handler_table
 
-	/* retrieve SRR0/SRR1 */
-	mf\_SRR0	r4
-	mf\_SRR1	r5
+	/* Load handler address */
+	lwzx	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0, SCRATCH_REGISTER_1
 
 	/*
-	 * branch to common routine;
+	 * First parameter = exception frame pointer + FRAME_LINK_SPACE
 	 *
-	 * r1, r3, r4, r5, cr, lr and r14 are saved on the
-	 * stack at this point.
+	 * We add FRAME_LINK_SPACE to the frame pointer because the high level
+	 * handler expects a BSP_Exception_frame structure.
 	 */
-	bl		wrap_common
+	addi	r3, FRAME_REGISTER, FRAME_LINK_SPACE 
 
-	/* 
-	 * restore SRRs, r4, r5, r1 (stack pointer) and lr;
-	 * wrap_common restores r3, r14 and cr for us.
+	/*
+	 * Second parameter = vector number (r4 is the VECTOR_REGISTER)
 	 *
-	 * NOTE: we restore r1 from the frame rather than
-	 * just popping (adding to current r1) since the
-	 * exception handler might have done strange things
-	 * (e.g., a debugger moving and relocating the stack).
+	 * Discard the exception type and store the vector number
+	 * in the vector register.  Take only the least significant
+	 * five bits (= LAST_VALID_EXC + 1).
+	 */
+	rlwinm	VECTOR_REGISTER, VECTOR_REGISTER, 0, 27, 31
+
+	/* Call handler */
+	mtctr	SCRATCH_REGISTER_0
+	bctrl
+
+	/* Check return value and call global handler if necessary */
+	cmpwi   r3, 0
+	bne	wrap_call_global_handler_\_FLVR
+
+wrap_handler_done_\_FLVR:
+
+	/* Check if exception is recoverable */
+	RECOVER_CHECK_\_PRI	_FLVR=\_FLVR
+
+	/*
+	 * Depending on the exception type we do now restore the non-volatile
+	 * registers or enable thread dispatching and switch back from the ISR
+	 * stack.
+	 */
+
+	/* Branch for synchronous exceptions */
+	bge	CR_TYPE, wrap_restore_non_volatile_regs_\_FLVR
+
+	/*
+	 * Switch back to original stack (FRAME_REGISTER == r1 if we are still
+	 * on the IRQ stack).
+	 */
+	mr	r1, FRAME_REGISTER
+
+	/*
+	 * Check thread dispatch disable level AND lower priority locks (in
+	 * CR_LOCK): ONLY if the thread dispatch disable level == 0 AND no lock
+	 * is set then call ppc_exc_wrapup() which may do a context switch.  We
+	 * can skip TEST_LOCK, because it has no side effects.
+	 */
+
+	/* Decrement ISR nest level and thread dispatch disable level */
+	lwz	SCRATCH_REGISTER_0, _ISR_Nest_level@sdarel(r13)
+	lwz	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+	subi	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0, 1
+	subic.	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, 1
+	stw	SCRATCH_REGISTER_0, _ISR_Nest_level@sdarel(r13)
+	stw	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+
+	/* Branch to skip thread dispatching */
+	bne	wrap_thread_dispatching_done_\_FLVR
+
+	/* Test lower-priority locks (result in non-volatile CR_LOCK) */
+	TEST_LOCK_\_PRI _FLVR=\_FLVR
+
+	/* Branch to skip thread dispatching */
+	bne	CR_LOCK, wrap_thread_dispatching_done_\_FLVR
+
+	/* Load address of ppc_exc_wrapup() */
+	LA	SCRATCH_REGISTER_0, ppc_exc_wrapup
+
+	/* First parameter = exception frame pointer + FRAME_LINK_SPACE */
+	addi	r3, FRAME_REGISTER, FRAME_LINK_SPACE 
+
+	/* Call ppc_exc_wrapup() */
+	mtctr	SCRATCH_REGISTER_0
+	bctrl
+
+wrap_thread_dispatching_done_\_FLVR:
+
+	/* Restore MSR? */
+	bne	CR_MSR, wrap_restore_msr_\_FLVR
+
+wrap_restore_msr_done_\_FLVR:
+
+	/*
+	 * At this point r1 is a valid exception frame pointer and
+	 * FRAME_REGISTER is no longer needed.
+	 */
+
+	/* Restore frame register */
+	lwz	FRAME_REGISTER, FRAME_OFFSET(r1)
+
+	/* Restore XER and CTR */
+	lwz	SCRATCH_REGISTER_0, EXC_XER_OFFSET(r1) 
+	lwz	SCRATCH_REGISTER_1, EXC_CTR_OFFSET(r1)
+	mtxer	SCRATCH_REGISTER_0
+	mtctr	SCRATCH_REGISTER_1
+
+	/* Restore CR and LR */
+	lwz	SCRATCH_REGISTER_0, EXC_CR_OFFSET(r1)
+	lwz	SCRATCH_REGISTER_1, EXC_LR_OFFSET(r1)
+	mtcr	SCRATCH_REGISTER_0
+	mtlr	SCRATCH_REGISTER_1
+
+	/* Restore volatile registers */
+	lwz	r0, GPR0_OFFSET(r1)
+	lwz	r3, GPR3_OFFSET(r1)
+	lwz	r8, GPR8_OFFSET(r1)
+	lwz	r9, GPR9_OFFSET(r1)
+	lwz	r10, GPR10_OFFSET(r1)
+	lwz	r11, GPR11_OFFSET(r1)
+	lwz	r12, GPR12_OFFSET(r1)
+
+	/* Restore read-only small data area anchor (EABI) */
+	lwz	r2, GPR2_OFFSET(r1)
+
+	/* Restore vector register */
+	lwz	VECTOR_REGISTER, VECTOR_OFFSET(r1)
+
+	/*
+	 * Disable all asynchronous exceptions which can do a thread dispatch.
+	 * See README.
 	 */
-	mt\_SRR0	r4
-	mt\_SRR1	r5
-	/* restore lr */
-	lwz		r5,  EXC_LR_OFFSET(r1)
-	lwz		r4,  GPR4_OFFSET(r1)
-	mtlr	r5
-	lwz		r5,  GPR5_OFFSET(r1)
-	lwz		r1,  EXC_MIN_GPR1(r1)
+	INTERRUPT_DISABLE	SCRATCH_REGISTER_1, SCRATCH_REGISTER_0
+
+	/* Restore scratch registers and SRRs */
+	lwz	SCRATCH_REGISTER_0, SRR0_FRAME_OFFSET(r1)
+	lwz	SCRATCH_REGISTER_1, SRR1_FRAME_OFFSET(r1)
+	lwz	SCRATCH_REGISTER_2, SCRATCH_REGISTER_2_OFFSET(r1)
+	mtspr	\_SRR0, SCRATCH_REGISTER_0
+	lwz	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0_OFFSET(r1)
+	mtspr	\_SRR1, SCRATCH_REGISTER_1
+	lwz	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1_OFFSET(r1)
+
+	/*
+	 * We restore r1 from the frame rather than just popping (adding to
+	 * current r1) since the exception handler might have done strange
+	 * things (e.g. a debugger moving and relocating the stack).
+	 */
+	lwz	r1, 0(r1)
+
+	/* Return */
 	\_RFI
+
+wrap_change_msr_\_FLVR:
+
+	mfmsr	SCRATCH_REGISTER_1
+	or	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, SCRATCH_REGISTER_0
+	mtmsr	SCRATCH_REGISTER_1
+	msync
+	isync
+	b	wrap_change_msr_done_\_FLVR
+
+wrap_restore_msr_\_FLVR:
+
+	lwz	SCRATCH_REGISTER_0, ppc_exc_msr_bits@sdarel(r13)
+	mfmsr	SCRATCH_REGISTER_1
+	andc	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, SCRATCH_REGISTER_0
+	mtmsr	SCRATCH_REGISTER_1
+	msync
+	isync
+	b	wrap_restore_msr_done_\_FLVR
+
+wrap_save_non_volatile_regs_\_FLVR:
+
+	/* Load pristine stack pointer */
+	lwz	SCRATCH_REGISTER_1, 0(FRAME_REGISTER)
+
+	/* Save small data area anchor (SYSV) */
+	stw	r13, GPR13_OFFSET(FRAME_REGISTER)
+
+	/* Save pristine stack pointer */
+	stw	SCRATCH_REGISTER_1, GPR1_OFFSET(FRAME_REGISTER)
+
+	/* r14 is the FRAME_REGISTER and will be saved elsewhere */
+
+	/* Save non-volatile registers r15 .. r31 */
+	stmw	r15, GPR15_OFFSET(FRAME_REGISTER)
+
+	b	wrap_disable_thread_dispatching_done_\_FLVR
+
+wrap_restore_non_volatile_regs_\_FLVR:
+
+	/* Load stack pointer */
+	lwz	SCRATCH_REGISTER_0, GPR1_OFFSET(r1)
+
+	/* Restore small data area anchor (SYSV) */
+	lwz	r13, GPR13_OFFSET(r1)
+	
+	/* r14 is the FRAME_REGISTER and will be restored elsewhere */
+
+	/* Restore non-volatile registers r15 .. r31 */
+	lmw	r15, GPR15_OFFSET(r1)
+
+	/* Restore stack pointer */
+	stw	SCRATCH_REGISTER_0, 0(r1)
+
+	b	wrap_thread_dispatching_done_\_FLVR
+
+wrap_call_global_handler_\_FLVR:
+
+	/* First parameter = exception frame pointer + FRAME_LINK_SPACE */
+	addi	r3, FRAME_REGISTER, FRAME_LINK_SPACE 
+
+	/* Load global handler address */
+	LW	SCRATCH_REGISTER_0, globalExceptHdl
+
+	/* Check address */
+	cmpwi	SCRATCH_REGISTER_0, 0
+	beq	wrap_handler_done_\_FLVR
+
+	/* Call global handler */
+	mtctr	SCRATCH_REGISTER_0
+	bctrl
+
+	b	wrap_handler_done_\_FLVR
+
 	.endm