4 files changed, 33 insertions, 9 deletions
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)