* Patch by Hans-Joerg Frieden, 06 Dec 2002

  Fix misc problems with AmigaOne support

* Patch by Chris Hallinan, 3 Dec 2002:
  minor cleanup to the MPC8245 EPIC driver

* Patch by Pierre Aubert , 28 Nov 2002
  Add support for external (SIU) interrupts on MPC8xx

* Patch by Pierre Aubert , 28 Nov 2002
  Fix nested syscalls bug in standalone applications

* Patch by David Müller, 27 Nov 2002:
  fix output of "pciinfo" command for CardBus bridge devices.

* Fix bug in TQM8260 board detection - boards got stuck when board ID
  was not readable

9 files changed, 239 insertions, 119 deletions

diff --git a/cpu/74xx_7xx/start.S b/cpu/74xx_7xx/start.S
index fe08f8e..0d63144 100644
--- a/cpu/74xx_7xx/start.S
+++ b/cpu/74xx_7xx/start.S
@@ -177,7 +177,14 @@ SystemCall:
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r12,0xd00-4*3		/* save LR & SRRx */
+	li	r20,0xd00-4		/* Get stack pointer */
+	lwz	r12,0(r20)
+	subi	r12,r12,12		/* Adjust stack pointer */
+	li	r0,0xc00+_end_back-SystemCall
+	cmplw	0, r0, r12		/* Check stack overflow */
+	bgt	1f
+	stw	r12,0(r20)
+
 	mflr	r0
 	stw	r0,0(r12)
 	mfspr	r0,SRR0
@@ -202,7 +209,9 @@ _back:
 	mtmsr	r11
 	SYNC
 
-	li	r12,0xd00-4*3		/* restore regs */
+	li	r12,0xd00-4		/* restore regs */
+	lwz	r12,0(r12)
+
 	lwz	r11,0(r12)
 	mtlr	r11
 	lwz	r11,4(r12)
@@ -210,8 +219,13 @@ _back:
 	lwz	r11,8(r12)
 	mtspr	SRR1,r11
 
+	addi	r12,r12,12		/* Adjust stack pointer */
+	li	r20,0xd00-4
+	stw	r12,0(r20)
+
 	SYNC
 	rfi
+_end_back:
 
 	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
 
@@ -716,6 +730,9 @@ in_ram:
 	bne	5b
 6:
 	mr	r3, r10		/* Destination Address		*/
+#ifdef CONFIG_AMIGAONEG3SE
+	mr	r4, r9		/* Use RAM copy of the global data */
+#endif
 	bl	after_reloc
 
 	/* not reached - end relocate_code */
diff --git a/cpu/mpc824x/drivers/epic/epic.h b/cpu/mpc824x/drivers/epic/epic.h
index 7751db2..17e4afb 100644
--- a/cpu/mpc824x/drivers/epic/epic.h
+++ b/cpu/mpc824x/drivers/epic/epic.h
@@ -108,6 +108,9 @@
 #define EPIC_PROC_INT_ACK_REG (EPIC_EUMBBAR + 0x200a0)/* Int. acknowledge */
 #define EPIC_PROC_EOI_REG (EPIC_EUMBBAR + 0x200b0)/* End of interrupt */
 
+#define EPIC_VEC_PRI_MASK      0x80000000 /* Mask Interrupt bit in IVPR */
+#define EPIC_VEC_PRI_DFLT_PRI  8          /* Interrupt Priority in IVPR */
+
 /* Error code */
 
 #define OK       0
diff --git a/cpu/mpc824x/drivers/epic/epic1.c b/cpu/mpc824x/drivers/epic/epic1.c
index 362e129..eb7ed40 100644
--- a/cpu/mpc824x/drivers/epic/epic1.c
+++ b/cpu/mpc824x/drivers/epic/epic1.c
@@ -70,6 +70,10 @@ void epicInit
     tmp = sysEUMBBARRead(EPIC_GLOBAL_REG);
     tmp |= 0xa0000000;                  /* Set the Global Conf. register */
     sysEUMBBARWrite(EPIC_GLOBAL_REG, tmp);
+	/*
+	 * Wait for EPIC to reset - CLH
+	 */
+    while( (sysEUMBBARRead(EPIC_GLOBAL_REG) & 0x80000000) == 1);
     sysEUMBBARWrite(EPIC_GLOBAL_REG, 0x20000000);
     tmp = sysEUMBBARRead(EPIC_INT_CONF_REG);    /* Read interrupt conf. reg */
 
@@ -81,7 +85,8 @@ void epicInit
         sysEUMBBARWrite(EPIC_INT_CONF_REG, tmp);
         }
 
-    while (epicIntAck() != 0xff);       /* Clear all pending interrupts */
+    while (epicIntAck() != 0xff)       /* Clear all pending interrupts */
+		epicEOI();
 }
 
 /****************************************************************************
@@ -92,18 +97,18 @@ void epicInit
  *
  *  RETURNS:  None
  */
-void epicIntEnable
-    (
-    int intVec        /* Interrupt Vector Number */
-    )
-    {
+void epicIntEnable(int intVec)
+{
     ULONG tmp;
     ULONG srAddr;
 
     srAddr = SrcVecTable[intVec].srcAddr;  /* Retrieve src Vec/Prio register */
     tmp = sysEUMBBARRead(srAddr);
-    tmp &= 0x7fffffff;             /* Clear the mask bit */
+    tmp &= ~EPIC_VEC_PRI_MASK;             /* Clear the mask bit */
+    tmp |= (EPIC_VEC_PRI_DFLT_PRI << 16);   /* Set priority to Default - CLH */
+    tmp |= intVec;				           /* Set Vector number */
     sysEUMBBARWrite(srAddr, tmp);
+
     return;
     }
 
diff --git a/cpu/mpc824x/interrupts.c b/cpu/mpc824x/interrupts.c
index 2011671..12841b3 100644
--- a/cpu/mpc824x/interrupts.c
+++ b/cpu/mpc824x/interrupts.c
@@ -92,6 +92,8 @@ int interrupt_init (void)
 	 */
 
 	epicInit (EPIC_DIRECT_IRQ, 0);
+	/* EPIC won't generate INT unless Current Task Pri < 15 */
+	epicCurTaskPrioSet(0);
 
 	set_dec (decrementer_count);
 
diff --git a/cpu/mpc824x/start.S b/cpu/mpc824x/start.S
index bd9706d..18b8e61 100644
--- a/cpu/mpc824x/start.S
+++ b/cpu/mpc824x/start.S
@@ -278,7 +278,14 @@ SystemCall:
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r12,0xd00-4*3		/* save LR & SRRx */
+	li	r20,0xd00-4		/* Get stack pointer */
+	lwz	r12,0(r20)
+	subi	r12,r12,12		/* Adjust stack pointer */
+	li	r0,0xc00+_end_back-SystemCall
+	cmplw	0, r0, r12		/* Check stack overflow */
+	bgt	1f
+	stw	r12,0(r20)
+
 	mflr	r0
 	stw	r0,0(r12)
 	mfspr	r0,SRR0
@@ -303,7 +310,9 @@ _back:
 	mtmsr	r11
 	SYNC
 
-	li	r12,0xd00-4*3		/* restore regs */
+	li	r12,0xd00-4		/* restore regs */
+	lwz	r12,0(r12)
+
 	lwz	r11,0(r12)
 	mtlr	r11
 	lwz	r11,4(r12)
@@ -311,8 +320,13 @@ _back:
 	lwz	r11,8(r12)
 	mtspr	SRR1,r11
 
+	addi	r12,r12,12		/* Adjust stack pointer */
+	li	r20,0xd00-4
+	stw	r12,0(r20)
+
 	SYNC
 	rfi
+_end_back:
 
 	STD_EXCEPTION(EXC_OFF_TRACE, SingleStep, UnknownException)
 
diff --git a/cpu/mpc8260/start.S b/cpu/mpc8260/start.S
index 10a8988..d79c578 100644
--- a/cpu/mpc8260/start.S
+++ b/cpu/mpc8260/start.S
@@ -324,7 +324,14 @@ SystemCall:
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r12,0xd00-4*3		/* save LR & SRRx */
+	li	r20,0xd00-4		/* Get stack pointer */
+	lwz	r12,0(r20)
+	subi	r12,r12,12		/* Adjust stack pointer */
+	li	r0,0xc00+_end_back-SystemCall
+	cmplw	0, r0, r12		/* Check stack overflow */
+	bgt	1f
+	stw	r12,0(r20)
+
 	mflr	r0
 	stw	r0,0(r12)
 	mfspr	r0,SRR0
@@ -349,7 +356,9 @@ _back:
 	mtmsr	r11
 	SYNC
 
-	li	r12,0xd00-4*3		/* restore regs */
+	li	r12,0xd00-4		/* restore regs */
+	lwz	r12,0(r12)
+
 	lwz	r11,0(r12)
 	mtlr	r11
 	lwz	r11,4(r12)
@@ -357,8 +366,13 @@ _back:
 	lwz	r11,8(r12)
 	mtspr	SRR1,r11
 
+	addi	r12,r12,12		/* Adjust stack pointer */
+	li	r20,0xd00-4
+	stw	r12,0(r20)
+
 	SYNC
 	rfi
+_end_back:
 
 	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
 
diff --git a/cpu/mpc8xx/interrupts.c b/cpu/mpc8xx/interrupts.c
index 57ff4dd..8664826 100644
--- a/cpu/mpc8xx/interrupts.c
+++ b/cpu/mpc8xx/interrupts.c
@@ -28,99 +28,104 @@
 #include <asm/processor.h>
 #include <commproc.h>
 
-/****************************************************************************/
+/************************************************************************/
 
-unsigned decrementer_count;		/* count value for 1e6/HZ microseconds */
+unsigned decrementer_count;	/* count value for 1e6/HZ microseconds	*/
 
-/****************************************************************************/
+/************************************************************************/
 
 /*
  * CPM interrupt vector functions.
  */
-struct	cpm_action {
-	 interrupt_handler_t *handler;
-	 void *arg;
+struct interrupt_action {
+	interrupt_handler_t *handler;
+	void *arg;
 };
 
-static struct cpm_action cpm_vecs[CPMVEC_NR];
+static struct interrupt_action cpm_vecs[CPMVEC_NR];
+static struct interrupt_action irq_vecs[NR_IRQS];
 
 static void cpm_interrupt_init (void);
-static void cpm_interrupt(int irq, struct pt_regs * regs);
+static void cpm_interrupt (void *regs);
 
-/****************************************************************************/
+/************************************************************************/
 
-static __inline__ unsigned long get_msr(void)
+static __inline__ unsigned long get_msr (void)
 {
-    unsigned long msr;
+	unsigned long msr;
 
-    asm volatile("mfmsr %0" : "=r" (msr) :);
-    return msr;
+	asm volatile ("mfmsr %0":"=r" (msr):);
+
+	return msr;
 }
 
-static __inline__ void set_msr(unsigned long msr)
+static __inline__ void set_msr (unsigned long msr)
 {
-    asm volatile("mtmsr %0" : : "r" (msr));
+	asm volatile ("mtmsr %0"::"r" (msr));
 }
 
-static __inline__ unsigned long get_dec(void)
+static __inline__ unsigned long get_dec (void)
 {
-    unsigned long val;
+	unsigned long val;
+
+	asm volatile ("mfdec %0":"=r" (val):);
 
-    asm volatile("mfdec %0" : "=r" (val) :);
-    return val;
+	return val;
 }
 
 
-static __inline__ void set_dec(unsigned long val)
+static __inline__ void set_dec (unsigned long val)
 {
-    asm volatile("mtdec %0" : : "r" (val));
+	asm volatile ("mtdec %0"::"r" (val));
 }
 
 
 void enable_interrupts (void)
 {
-	set_msr (get_msr() | MSR_EE);
+	set_msr (get_msr () | MSR_EE);
 }
 
 /* returns flag if MSR_EE was set before */
 int disable_interrupts (void)
 {
-	ulong msr = get_msr();
+	ulong msr = get_msr ();
+
 	set_msr (msr & ~MSR_EE);
 	return ((msr & MSR_EE) != 0);
 }
 
-/****************************************************************************/
+/************************************************************************/
 
-int interrupt_init(void)
+int interrupt_init (void)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
 
-	decrementer_count = get_tbclk() / CFG_HZ;
+	decrementer_count = get_tbclk () / CFG_HZ;
 
-	cpm_interrupt_init();
+	/* disable all interrupts */
+	immr->im_siu_conf.sc_simask = 0;
 
-	/* disable all interrupts except for the CPM interrupt */
-	immr->im_siu_conf.sc_simask = 1 << (31-CPM_INTERRUPT);
+	/* Configure CPM interrupts */
+	cpm_interrupt_init ();
 
 	set_dec (decrementer_count);
 
-	set_msr (get_msr() | MSR_EE);
+	set_msr (get_msr () | MSR_EE);
 
 	return (0);
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 /*
  * Handle external interrupts
  */
-void external_interrupt(struct pt_regs *regs)
+void external_interrupt (struct pt_regs *regs)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
-	int	irq;
-	ulong	simask, newmask;
-	ulong	vec, v_bit;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+	int irq;
+	ulong simask, newmask;
+	ulong vec, v_bit;
 
 	/*
 	 * read the SIVEC register and shift the bits down
@@ -137,14 +142,15 @@ void external_interrupt(struct pt_regs *regs)
 	newmask = simask & (~(0xFFFF0000 >> irq));
 	immr->im_siu_conf.sc_simask = newmask;
 
-	if (!(irq & 0x1)) {			/* External Interrupt ?		*/
+	if (!(irq & 0x1)) {		/* External Interrupt ?     */
 		ulong siel;
+
 		/*
 		 * Read Interrupt Edge/Level Register
 		 */
 		siel = immr->im_siu_conf.sc_siel;
 
-		if (siel & v_bit) {		/* edge triggered interrupt ?	*/
+		if (siel & v_bit) {	/* edge triggered interrupt ?   */
 			/*
 			 * Rewrite SIPEND Register to clear interrupt
 			 */
@@ -152,34 +158,29 @@ void external_interrupt(struct pt_regs *regs)
 		}
 	}
 
-	switch (irq) {
-	case CPM_INTERRUPT:
-		cpm_interrupt (irq, regs);
-		break;
-	default:
+	if (irq_vecs[irq].handler != NULL) {
+		irq_vecs[irq].handler (irq_vecs[irq].arg);
+	} else {
 		printf ("\nBogus External Interrupt IRQ %d Vector %ld\n",
-			irq, vec);
+				irq, vec);
 		/* turn off the bogus interrupt to avoid it from now */
 		simask &= ~v_bit;
-		break;
 	}
-
 	/*
 	 * Re-Enable old Interrupt Mask
 	 */
 	immr->im_siu_conf.sc_simask = simask;
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 /*
  * CPM interrupt handler
  */
-static void
-cpm_interrupt(int irq, struct pt_regs * regs)
+static void cpm_interrupt (void *regs)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
-	uint	vec;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+	uint vec;
 
 	/*
 	 * Get the vector by setting the ACK bit
@@ -190,13 +191,14 @@ cpm_interrupt(int irq, struct pt_regs * regs)
 	vec >>= 11;
 
 	if (cpm_vecs[vec].handler != NULL) {
-		(*cpm_vecs[vec].handler)(cpm_vecs[vec].arg);
+		(*cpm_vecs[vec].handler) (cpm_vecs[vec].arg);
 	} else {
 		immr->im_cpic.cpic_cimr &= ~(1 << vec);
 		printf ("Masking bogus CPM interrupt vector 0x%x\n", vec);
 	}
 	/*
-	 * After servicing the interrupt, we have to remove the status indicator.
+	 * After servicing the interrupt,
+	 * we have to remove the status indicator.
 	 */
 	immr->im_cpic.cpic_cisr |= (1 << vec);
 }
@@ -207,75 +209,110 @@ cpm_interrupt(int irq, struct pt_regs * regs)
  * to do is ACK it and return. This is a no-op function so we don't
  * need any special tests in the interrupt handler.
  */
-static void
-cpm_error_interrupt (void *dummy)
+static void cpm_error_interrupt (void *dummy)
 {
 }
 
-/****************************************************************************/
-
+/************************************************************************/
 /*
- * Install and free a CPM interrupt handler.
+ * Install and free an interrupt handler
  */
-
-void
-irq_install_handler(int vec, interrupt_handler_t *handler, void *arg)
+void irq_install_handler (int vec, interrupt_handler_t * handler,
+						  void *arg)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
-
-	if (cpm_vecs[vec].handler != NULL) {
-		printf ("CPM interrupt 0x%x replacing 0x%x\n",
-			(uint)handler, (uint)cpm_vecs[vec].handler);
-	}
-	cpm_vecs[vec].handler = handler;
-	cpm_vecs[vec].arg     = arg;
-	immr->im_cpic.cpic_cimr |= (1 << vec);
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+
+	if ((vec & CPMVEC_OFFSET) != 0) {
+		/* CPM interrupt */
+		vec &= 0xffff;
+		if (cpm_vecs[vec].handler != NULL) {
+			printf ("CPM interrupt 0x%x replacing 0x%x\n",
+				(uint) handler,
+				(uint) cpm_vecs[vec].handler);
+		}
+		cpm_vecs[vec].handler = handler;
+		cpm_vecs[vec].arg = arg;
+		immr->im_cpic.cpic_cimr |= (1 << vec);
+#if 0
+		printf ("Install CPM interrupt for vector %d ==> %p\n",
+			vec, handler);
+#endif
+	} else {
+		/* SIU interrupt */
+		if (irq_vecs[vec].handler != NULL) {
+			printf ("SIU interrupt %d 0x%x replacing 0x%x\n",
+				vec,
+				(uint) handler,
+				(uint) cpm_vecs[vec].handler);
+		}
+		irq_vecs[vec].handler = handler;
+		irq_vecs[vec].arg = arg;
+		immr->im_siu_conf.sc_simask |= 1 << (31 - vec);
 #if 0
-	printf ("Install CPM interrupt for vector %d ==> %p\n", vec, handler);
+		printf ("Install SIU interrupt for vector %d ==> %p\n",
+			vec, handler);
 #endif
+	}
 }
 
-void
-irq_free_handler(int vec)
+void irq_free_handler (int vec)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+
+	if ((vec & CPMVEC_OFFSET) != 0) {
+		/* CPM interrupt */
+		vec &= 0xffff;
+#if 0
+		printf ("Free CPM interrupt for vector %d ==> %p\n",
+			vec, cpm_vecs[vec].handler);
+#endif
+		immr->im_cpic.cpic_cimr &= ~(1 << vec);
+		cpm_vecs[vec].handler = NULL;
+		cpm_vecs[vec].arg = NULL;
+	} else {
+		/* SIU interrupt */
 #if 0
-	printf ("Free CPM interrupt for vector %d ==> %p\n",
-		vec, cpm_vecs[vec].handler);
+		printf ("Free CPM interrupt for vector %d ==> %p\n",
+			vec, cpm_vecs[vec].handler);
 #endif
-	immr->im_cpic.cpic_cimr &= ~(1 << vec);
-	cpm_vecs[vec].handler = NULL;
-	cpm_vecs[vec].arg     = NULL;
+		immr->im_siu_conf.sc_simask &= ~(1 << (31 - vec));
+		irq_vecs[vec].handler = NULL;
+		irq_vecs[vec].arg = NULL;
+	}
 }
 
-/****************************************************************************/
+/************************************************************************/
 
-static void
-cpm_interrupt_init (void)
+static void cpm_interrupt_init (void)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
 
 	/*
 	 * Initialize the CPM interrupt controller.
 	 */
 
 	immr->im_cpic.cpic_cicr =
-		( CICR_SCD_SCC4 |
-		  CICR_SCC_SCC3 |
-		  CICR_SCB_SCC2 |
-		  CICR_SCA_SCC1 ) | ((CPM_INTERRUPT/2) << 13) | CICR_HP_MASK;
+		(CICR_SCD_SCC4 |
+		 CICR_SCC_SCC3 |
+		 CICR_SCB_SCC2 |
+		 CICR_SCA_SCC1) | ((CPM_INTERRUPT / 2) << 13) | CICR_HP_MASK;
 
 	immr->im_cpic.cpic_cimr = 0;
 
 	/*
 	 * Install the error handler.
 	 */
-	irq_install_handler(CPMVEC_ERROR, cpm_error_interrupt, NULL);
+	irq_install_handler (CPMVEC_ERROR, cpm_error_interrupt, NULL);
 
 	immr->im_cpic.cpic_cicr |= CICR_IEN;
+
+	/*
+	 * Install the cpm interrupt handler
+	 */
+	irq_install_handler (CPM_INTERRUPT, cpm_interrupt, NULL);
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 volatile ulong timestamp = 0;
 
@@ -284,18 +321,19 @@ volatile ulong timestamp = 0;
  * with interrupts disabled.
  * Trivial implementation - no need to be really accurate.
  */
-void timer_interrupt(struct pt_regs *regs)
+void timer_interrupt (struct pt_regs *regs)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+
 #ifdef CONFIG_STATUS_LED
-    	extern void status_led_tick (ulong);
+	extern void status_led_tick (ulong);
 #endif
 #if 0
 	printf ("*** Timer Interrupt *** ");
 #endif
 	/* Reset Timer Expired and Timers Interrupt Status */
 	immr->im_clkrstk.cark_plprcrk = KAPWR_KEY;
-	__asm__("nop");
+	__asm__ ("nop");
 	immr->im_clkrst.car_plprcr |= PLPRCR_TEXPS | PLPRCR_TMIST;
 	/* Restore Decrementer Count */
 	set_dec (decrementer_count);
@@ -304,11 +342,10 @@ void timer_interrupt(struct pt_regs *regs)
 
 #ifdef CONFIG_STATUS_LED
 	status_led_tick (timestamp);
-#endif	/* CONFIG_STATUS_LED */
+#endif /* CONFIG_STATUS_LED */
 
 #if defined(CONFIG_WATCHDOG) || defined(CFG_CMA_LCD_HEARTBEAT)
 
-
 	/*
 	 * The shortest watchdog period of all boards (except LWMON)
 	 * is approx. 1 sec, thus re-trigger watchdog at least
@@ -321,20 +358,20 @@ void timer_interrupt(struct pt_regs *regs)
 #endif
 
 #if defined(CFG_CMA_LCD_HEARTBEAT)
-		extern void lcd_heartbeat(void);
-		lcd_heartbeat();
+		extern void lcd_heartbeat (void);
+
+		lcd_heartbeat ();
 #endif /* CFG_CMA_LCD_HEARTBEAT */
 
 #if defined(CONFIG_WATCHDOG)
-		reset_8xx_watchdog(immr);
+		reset_8xx_watchdog (immr);
 #endif /* CONFIG_WATCHDOG */
 
 	}
-
 #endif /* CONFIG_WATCHDOG || CFG_CMA_LCD_HEARTBEAT */
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 void reset_timer (void)
 {
@@ -351,4 +388,4 @@ void set_timer (ulong t)
 	timestamp = t;
 }
 
-/****************************************************************************/
+/************************************************************************/
diff --git a/cpu/mpc8xx/start.S b/cpu/mpc8xx/start.S
index a430061..a1b7eff 100644
--- a/cpu/mpc8xx/start.S
+++ b/cpu/mpc8xx/start.S
@@ -285,7 +285,14 @@ SystemCall:
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r12,0xd00-4*3		/* save LR & SRRx */
+	li	r20,0xd00-4		/* Get stack pointer */
+	lwz	r12,0(r20)
+	subi	r12,r12,12		/* Adjust stack pointer */
+	li	r0,0xc00+_end_back-SystemCall
+	cmplw	0, r0, r12		/* Check stack overflow */
+	bgt	1f
+	stw	r12,0(r20)
+
 	mflr	r0
 	stw	r0,0(r12)
 	mfspr	r0,SRR0
@@ -310,7 +317,9 @@ _back:
 	mtmsr	r11
 	SYNC
 
-	li	r12,0xd00-4*3		/* restore regs */
+	li	r12,0xd00-4		/* restore regs */
+	lwz	r12,0(r12)
+
 	lwz	r11,0(r12)
 	mtlr	r11
 	lwz	r11,4(r12)
@@ -318,8 +327,13 @@ _back:
 	lwz	r11,8(r12)
 	mtspr	SRR1,r11
 
+	addi	r12,r12,12		/* Adjust stack pointer */
+	li	r20,0xd00-4
+	stw	r12,0(r20)
+
 	SYNC
 	rfi
+_end_back:
 
 	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
 
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index baaaba4..c40a8db 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -775,7 +775,14 @@ SystemCall:
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r12,0xd00-4*3		/* save LR & SRRx */
+	li	r20,0xd00-4		/* Get stack pointer */
+	lwz	r12,0(r20)
+	subi	r12,r12,12		/* Adjust stack pointer */
+	li	r0,0xc00+_end_back-SystemCall
+	cmplw	0, r0, r12		/* Check stack overflow */
+	bgt	1f
+	stw	r12,0(r20)
+
 	mflr	r0
 	stw	r0,0(r12)
 	mfspr	r0,SRR0
@@ -800,7 +807,9 @@ _back:
 	mtmsr	r11
 	SYNC
 
-	li	r12,0xd00-4*3		/* restore regs */
+	li	r12,0xd00-4		/* restore regs */
+	lwz	r12,0(r12)
+
 	lwz	r11,0(r12)
 	mtlr	r11
 	lwz	r11,4(r12)
@@ -808,8 +817,13 @@ _back:
 	lwz	r11,8(r12)
 	mtspr	SRR1,r11
 
+	addi	r12,r12,12		/* Adjust stack pointer */
+	li	r20,0xd00-4
+	stw	r12,0(r20)
+
 	SYNC
 	rfi
+_end_back:
 
 	STD_EXCEPTION(0xd00, SingleStep, UnknownException)