From 75de3da69e27f71c1b003321115609ad775772d8 Mon Sep 17 00:00:00 2001
From: hebe <devnull@localhost>
Date: Thu, 13 Sep 2012 10:16:02 +0200
Subject: [PATCH] Mcu, sleep fixes for mpc5668.

---
 arch/ppc/mpc55xx/drivers/Mcu.c        | 46 +++++++++++++++++++++++++--
 arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx |  7 ++--
 2 files changed, 49 insertions(+), 4 deletions(-)

diff --git a/arch/ppc/mpc55xx/drivers/Mcu.c b/arch/ppc/mpc55xx/drivers/Mcu.c
index 5b14c60c..178edfbf 100644
--- a/arch/ppc/mpc55xx/drivers/Mcu.c
+++ b/arch/ppc/mpc55xx/drivers/Mcu.c
@@ -689,6 +689,48 @@ void Mcu_PerformReset(void)
  */
 static void enterLowPower (Mcu_ModeType mcuMode )
 {
+#if defined(CFG_MPC5668)
+	uint32 timeout;
+	/* Set the sleep bit; following a WAIT instruction, the device will go to sleep */
+	CRP.PSCR.B.SLEEP = 1;
+
+	/* 0x1 32k, 0x2 64k, 0x3 128k -- RAMs maintain power */
+	CRP.PSCR.B.RAMSEL = 0x3;		// Keep all 128K
+
+	CRP.Z6VEC.R = (uint32)&McuE_LowPowerRecoverFlash;
+#if defined(CFG_VLE)
+	CRP.Z6VEC.VLE = 1;
+#endif
+
+	/* If we "Mcu_Wakeup()" is located in RAM, set FASTREC */
+	CRP.RECPTR.B.FASTREC = 0;
+
+	/* Halt everything */
+    SIU.HLT0.R = 0x037FFF3D;
+    SIU.HLT1.R = 0x18000F3C;
+    while((SIU.HLTACK0.R != 0x037FFF3D) && (SIU.HLTACK1.R != 0x18000F3C) && (timeout<3000)){}
+
+	/* put Z0 in reset if not used for wakeup */
+	CRP.Z0VEC.B.Z0RST = 1;
+
+	// TODO: Enable_all_internal_pull_devices (PULL_DOWN);
+
+	/* Save context and execute wait instruction.
+	 *
+	 * Things that matter here are
+	 * - Z1VEC, determines where TLB0 will point. TLB0 is written with a
+	 *   value at startup that 4K aligned to this address.
+	 * - LowPower_Sleep() will save a interrupt context so we will return
+	 *   intact.
+	 * - For devices with little RAM we don't want to impose the alignment
+	 *   requirements there. Almost as we have to occupy a 4K block for this..
+	 *   although the code does not take that much space.
+	 * */
+	McuE_EnterLowPower(mcuMode);
+
+    /* Clear sleep flags to allow pads to operate */
+	CRP.PSCR.B.SLEEPF = 0x1;
+#else
 	uint32 timeout;
 	/* Set the sleep bit; following a WAIT instruction, the device will go to sleep */
 	CRP.PSCR.B.SLEEP = 1;
@@ -730,7 +772,7 @@ static void enterLowPower (Mcu_ModeType mcuMode )
     /* Clear sleep flags to allow pads to operate */
     CRP.PSCR.B.SLEEPF = 0x1;
 
-
+#endif
 }
 
 
@@ -740,7 +782,7 @@ void Mcu_SetMode( Mcu_ModeType mcuMode)
 	// VALIDATE( ( McuMode <= Mcu_Global.config->McuNumberOfMcuModes ), MCU_SETMODE_SERVICE_ID, MCU_E_PARAM_MODE );
 
 
-#if defined(CFG_MPC5516)
+#if defined(CFG_MPC5516) || defined(CFG_MPC5668)
 	if( MCU_MODE_RUN == mcuMode ) {
 
 	} else if( MCU_MODE_SLEEP == mcuMode ) {
diff --git a/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx b/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx
index 485fa902..4b2532d5 100644
--- a/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx
+++ b/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx
@@ -11,8 +11,11 @@
 .section .text
 #endif
 
-
+#if defined(CFG_MPC5516) || defined(CFG_MPC5668)
 #define CRP_RECPTR 		0xfffec058
+#else
+#error
+#endif
 
 	.global McuE_EnterLowPower
 	
@@ -122,7 +125,7 @@ McuE_LowPowerRecoverFlash:
  #define VLE_VAL		0
  #endif
 
-#if defined(CFG_MPC5516)
+#if defined(CFG_MPC5516) || defined(CFG_MPC5668)
 #define SRAM_START			0x40000000
 #define FLASH_START			0x00000000
 #define PERIPHERAL_START	0xfff00000
-- 
2.39.2