From ab0f437f4f8b7fa1860125657c517b7b65bc1b01 Mon Sep 17 00:00:00 2001
From: mahi <devnull@localhost>
Date: Tue, 18 Sep 2012 10:02:44 +0200
Subject: [PATCH] Added Dma_DeInit()

---
 arch/ppc/mpc55xx/drivers/Dma.c |  9 +++++++++
 arch/ppc/mpc55xx/drivers/Dma.h |  1 +
 arch/ppc/mpc55xx/drivers/Mcu.c | 20 +++++++++++++-------
 3 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/arch/ppc/mpc55xx/drivers/Dma.c b/arch/ppc/mpc55xx/drivers/Dma.c
index 3787a6f2..9153fd04 100644
--- a/arch/ppc/mpc55xx/drivers/Dma.c
+++ b/arch/ppc/mpc55xx/drivers/Dma.c
@@ -48,6 +48,15 @@ void Dma_Init (const Dma_ConfigType *ConfigPtr)
   EDMA.CR.B.ERCA = ConfigPtr->dmaChannelArbitration;
 }
 
+void Dma_DeInit( void ) {
+	Dma_ChannelType channel;
+	for (channel = (Dma_ChannelType)0; channel < DMA_NUMBER_OF_CHANNELS; channel++)
+	{
+		Dma_StopChannel(channel);
+	}
+}
+
+
 void Dma_ConfigureChannel (Dma_TcdType *tcd, Dma_ChannelType channel)
 {
   /* Copy transfer configuration to correct channel. */
diff --git a/arch/ppc/mpc55xx/drivers/Dma.h b/arch/ppc/mpc55xx/drivers/Dma.h
index da43a2d0..dcea7a3b 100644
--- a/arch/ppc/mpc55xx/drivers/Dma.h
+++ b/arch/ppc/mpc55xx/drivers/Dma.h
@@ -346,6 +346,7 @@ extern const Dma_ConfigType DmaConfig [];
 
 
 void Dma_Init (const Dma_ConfigType *ConfigPtr);
+void Dma_DeInit (void );
 void Dma_ConfigureChannel (Dma_TcdType *tcd, Dma_ChannelType channel);
 void Dma_ConfigureChannelTranferSize (uint32_t nbrOfIterations, Dma_ChannelType channel);
 void Dma_ConfigureChannelSourceCorr (uint32_t sourceCorrection, Dma_ChannelType channel);
diff --git a/arch/ppc/mpc55xx/drivers/Mcu.c b/arch/ppc/mpc55xx/drivers/Mcu.c
index 1a8a4fbb..ab43113e 100644
--- a/arch/ppc/mpc55xx/drivers/Mcu.c
+++ b/arch/ppc/mpc55xx/drivers/Mcu.c
@@ -105,7 +105,7 @@ typedef void (*vfunc_t)();
 #if defined(CFG_MPC5516)
 static uint32 Mcu_SavedHaltFlags;
 #else
-#error Implement halt flags
+static uint32 Mcu_SavedHaltFlags[2];
 #endif
 
 
@@ -775,10 +775,8 @@ static void enterLowPower (Mcu_ModeType mcuMode )
 	/* Set Recover Vector */
 #if defined(CFG_MPC5516)
 
-
-
 	WRITE32(CRP_Z1VEC, ((uint32)&McuE_LowPowerRecoverFlash) | VLE_VAL );
-	READWRITE32(CRP_RECPTR,RECPTR_FASTREC,0 );
+	READWRITE32( CRP_RECPTR, RECPTR_FASTREC, 0 );
 
 	Mcu_SavedHaltFlags = SIU.HLT.R;
 	/* Halt everything */
@@ -792,6 +790,9 @@ static void enterLowPower (Mcu_ModeType mcuMode )
 
 	WRITE32(CRP_Z6VEC, ((uint32)&McuE_LowPowerRecoverFlash) | VLE_VAL );
 	READWRITE32(CRP_RECPTR,RECPTR_FASTREC,0 );
+
+	Mcu_SavedHaltFlags[0] = SIU.HLT0.R;
+	Mcu_SavedHaltFlags[1] = SIU.HLT1.R;
 	/* Halt everything */
     SIU.HLT0.R = 0x037FFF3D;
     SIU.HLT1.R = 0x18000F3C;
@@ -830,9 +831,14 @@ void Mcu_SetMode( Mcu_ModeType mcuMode)
 
 #if defined(CFG_MPC5516) || defined(CFG_MPC5668)
 	if( MCU_MODE_RUN == mcuMode ) {
-		/* Get Most back to "normal" halt flags */
-		SIU.HLT.R = Mcu_SavedHaltFlags;
 
+		/* Get back to "normal" halt flags */
+#if defined(CFG_MPC5516)
+		SIU.HLT.R = Mcu_SavedHaltFlags;
+#elif defined(CFG_MPC5668)
+		SIU.HLT0.R = Mcu_SavedHaltFlags[0];
+		SIU.HLT1.R = Mcu_SavedHaltFlags[1];
+#endif
 
 	} else if( MCU_MODE_SLEEP == mcuMode ) {
 		/*
@@ -840,7 +846,7 @@ void Mcu_SetMode( Mcu_ModeType mcuMode)
 		 *
 		 */
 #if defined(USE_DMA)
-		Dma_StopAll();
+		Dma_DeInit();
 #endif
 
 
-- 
2.39.2