From afc3ab33cd50eac9f10e9bc7007e11bb2a5f9a50 Mon Sep 17 00:00:00 2001
From: mahi <devnull@localhost>
Date: Tue, 4 Sep 2012 14:51:03 +0200
Subject: [PATCH] PPC: Sleep mode and wakeup pin now working

---
 arch/arm/arm_cm3/drivers/Mcu.c        |   1 -
 arch/ppc/mpc55xx/drivers/Mcu.c        |   8 +-
 arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx | 106 ++++++++++++++++++++------
 include/ppc/asm_ppc.h                 |  28 +++++++
 4 files changed, 119 insertions(+), 24 deletions(-)

diff --git a/arch/arm/arm_cm3/drivers/Mcu.c b/arch/arm/arm_cm3/drivers/Mcu.c
index 075ed34c..17b391f9 100644
--- a/arch/arm/arm_cm3/drivers/Mcu.c
+++ b/arch/arm/arm_cm3/drivers/Mcu.c
@@ -497,7 +497,6 @@ void Mcu_SetMode(const Mcu_ModeType McuMode)
   //VALIDATE( ( McuMode <= Mcu_Global.config->McuNumberOfMcuModes ), MCU_SETMODE_SERVICE_ID, MCU_E_PARAM_MODE );
   (void) McuMode;
 
-  /* NOT SUPPORTED */
 }
 
 //-------------------------------------------------------------------
diff --git a/arch/ppc/mpc55xx/drivers/Mcu.c b/arch/ppc/mpc55xx/drivers/Mcu.c
index 350f9183..dae6e09f 100644
--- a/arch/ppc/mpc55xx/drivers/Mcu.c
+++ b/arch/ppc/mpc55xx/drivers/Mcu.c
@@ -666,7 +666,10 @@ void Mcu_PerformReset(void)
 
 /**
  *
- *
+ * Application Notes!
+ * - AN3584, "MPC5510 Family Low Power Features"
+ *   Since it's not complete also check MPC5668
+ * - AN4150 , "Using Sleep Mode on the MPC5668x" and it's code
  *
  *
  * @param LPM
@@ -710,6 +713,9 @@ static void enterLowPower (Mcu_ModeType mcuMode )
 	 *   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;
 }
 
 
diff --git a/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx b/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx
index 8784f78c..aa6cb3f4 100644
--- a/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx
+++ b/arch/ppc/mpc55xx/drivers/Mcu_Sleep.sx
@@ -12,7 +12,7 @@
 #endif
 
 
-#define CRP_RECPTR 		0xfffe0058
+#define CRP_RECPTR 		0xfffec058
 
 	.global McuE_EnterLowPower
 	
@@ -27,7 +27,7 @@ McuE_EnterLowPower:
 	mfXER	r30
 	mfCTR	r31
 	stmw	r25, 0x78(r1)	#-- Store SPR data to stack
-	
+		
 	LOAD_ADDR_32(r4,CRP_RECPTR)
 	stw		r1,0x0(r4)	//  Save stack pointer to CRP to be preserved during LPM
 	.long	0x7C00007C	//  Wait instruction
@@ -72,34 +72,30 @@ McuE_LowPowerVector:
 	.global McuE_LowPowerRecoverFlash
 
 /*
- * When we come back here only TLB0 is setup.
- * - Map up the entire flash 
+ * When we come back here only TLB0 is setup for flash and then only 4K
  */	
 McuE_LowPowerRecoverFlash:
-	LOAD_ADDR_32(5,0x10030000)
-    mtspr SPR_MAS0,r5     
-    LOAD_ADDR_32(5,0xC0000500)
-    mtspr SPR_MAS1,r5     
-    LOAD_ADDR_32(5,0x40000008)
-    mtspr SPR_MAS2,r5     
-    LOAD_ADDR_32(5,0x4000003F)
-    mtspr SPR_MAS3,r5
-    tlbwe
-
-    LOAD_ADDR_32(r3,0xfffe8600+52)
-	stb		1,0(r3)	
     
+    bl cfg_MMU
+ 
+ 
+#if 0 
+ 	// Setup the SIU again.
+	li	 r4,0x0202
+    LOAD_ADDR_32(r3,0xfffe8040+(52*2))		// PD4, SIU_PCR 
+	sth  r4,0(r3)
+	
+	LOAD_ADDR_32(r3,0xfffe8600+52)		// PD4, SIU_GPDO
+	stb		1,0(r3)	
+
     // Release PAD keepers
-    LOAD_ADDR_32(r3,0xfffec060)
+    LOAD_ADDR_32(r3,0xfffec060)   // CRP_PSCR
     LOAD_ADDR_32(r4,0x1000)
     stw	 r4,0(r3)
-	    
-apa:    
-    b 	apa
+#endif    
     
 /* Recover the stack */
-    lis	r4,0xFFFE
-	ori	r4,r4,0xC058	#-- CRP.RECPRTR address
+	LOAD_ADDR_32(r4,CRP_RECPTR)
 	lwz	r1,0x0(r4)	#-- Restore stack pointer from CRP
 	lmw	r25,0x78(r1)	#-- Load SPR values back into GPRs
 	mtSRR1	r25
@@ -116,5 +112,71 @@ apa:
     /* branch back to Mcu_SetMode().. */
     blr
  
+ 
+ #if defined(CFG_VLE)
+ #define VLE_VAL		MAS2_VLE
+ #else
+ #define VLE_VAL		0
+ #endif
+
+#define SRAM_START			0x40000000
+#define FLASH_START			0x00000000
+#define PERIPHERAL_START	0xfff00000
+
+ 
+ cfg_MMU:
+
+#***************************************************/
+#     setup MMU                                    */
+#***************************************************/
+
+#TLB Entry 0 =  1M Internal flash 
+	LOAD_ADDR_32(5, 0x10000000 + (0<<16))
+    mtspr SPR_MAS0,r5     
+    LOAD_ADDR_32(5, 0xC0000000 + MAS1_TSIZE_4M )
+    mtspr SPR_MAS1,r5     
+    LOAD_ADDR_32(5, FLASH_START + VLE_VAL )
+    mtspr SPR_MAS2,r5     
+    LOAD_ADDR_32(5, FLASH_START + MAS3_FULL_ACCESS )
+    mtspr SPR_MAS3,r5
+    msync
+    isync
+	tlbwe
+	isync
+
+
+#TLB Entry 1 =  Peripheral bridge and BAM
+	LOAD_ADDR_32(5, 0x10000000 + (1<<16))
+    mtspr SPR_MAS0,r5     
+    LOAD_ADDR_32(5, 0xC0000000 + MAS1_TSIZE_1M)
+    mtspr SPR_MAS1,r5
+    LOAD_ADDR_32(5, PERIPHERAL_START + VLE_VAL +  MAS2_I)
+    mtspr SPR_MAS2,r5     
+    LOAD_ADDR_32(5, PERIPHERAL_START + MAS3_FULL_ACCESS )
+    mtspr SPR_MAS3,r5
+    msync
+    isync
+	tlbwe
+	isync
+
+
+#TLB Entry 2 =  External RAM. Skip this. 
+
+#TLB Entry 3 =  Internal SRAM
+	LOAD_ADDR_32(5, 0x10000000+(3<<16))
+    mtspr SPR_MAS0,r5     
+    LOAD_ADDR_32(5, 0xC0000000 + MAS1_TSIZE_256K )
+    mtspr SPR_MAS1,r5     
+    LOAD_ADDR_32(5, SRAM_START + VLE_VAL )
+    mtspr SPR_MAS2,r5     
+    LOAD_ADDR_32(5, SRAM_START + MAS3_FULL_ACCESS )
+    mtspr SPR_MAS3,r5
+    msync
+    isync
+	tlbwe
+	isync
+   	blr
+   	
+ 
 
 
diff --git a/include/ppc/asm_ppc.h b/include/ppc/asm_ppc.h
index 54cb6747..cd434b78 100644
--- a/include/ppc/asm_ppc.h
+++ b/include/ppc/asm_ppc.h
@@ -75,6 +75,34 @@
 
 #define INTC_SSCIR7 0xFFF48027
 
+/* MAS bits */
+#define MAS1_TSIZE_4K			(1<<8)
+#define MAS1_TSIZE_16K			(2<<8)
+#define MAS1_TSIZE_64K			(3<<8)
+#define MAS1_TSIZE_256K			(4<<8)
+#define MAS1_TSIZE_1M			(5<<8)
+#define MAS1_TSIZE_4M			(6<<8)
+#define MAS1_TSIZE_16M			(7<<8)
+#define MAS1_TSIZE_64M			(8<<8)
+#define MAS1_TSIZE_256M			(8<<9)
+
+#define MAS2_VLE	(1<<5)
+#define MAS2_W		(1<<4)
+#define MAS2_I		(1<<3)
+#define MAS2_M		(1<<2)
+#define MAS2_G		(1<<1)
+#define MAS2_E		(1<<0)
+
+#define MAS3_UX		(1<<5)
+#define MAS3_SX		(1<<4)
+#define MAS3_UW		(1<<3)
+#define MAS3_SW		(1<<2)
+#define MAS3_UR		(1<<1)
+#define MAS3_SR		(1<<0)
+
+#define MAS3_FULL_ACCESS (MAS3_UX+MAS3_UW+MAS3_UR+MAS3_SX+MAS3_SW+MAS3_SR)
+
+
 #if defined(_ASSEMBLER_)
 /*
  * PPC vs VLE assembler:
-- 
2.39.2