X-Git-Url: http://git.onelab.eu/?a=blobdiff_plain;f=arch%2Fpowerpc%2Fkernel%2Fidle_6xx.S;fp=arch%2Fpowerpc%2Fkernel%2Fidle_6xx.S;h=444fdcc769f1cf272bf84e7baa4bb497cc4e352f;hb=64ba3f394c830ec48a1c31b53dcae312c56f1604;hp=01bcd52bbf8e11f8594987f41a69a795a848071b;hpb=be1e6109ac94a859551f8e1774eb9a8469fe055c;p=linux-2.6.git

diff --git a/arch/powerpc/kernel/idle_6xx.S b/arch/powerpc/kernel/idle_6xx.S
index 01bcd52bb..444fdcc76 100644
--- a/arch/powerpc/kernel/idle_6xx.S
+++ b/arch/powerpc/kernel/idle_6xx.S
@@ -13,6 +13,7 @@
  *  2 of the License, or (at your option) any later version.
  */
 
+#include <linux/config.h>
 #include <linux/threads.h>
 #include <asm/reg.h>
 #include <asm/page.h>
@@ -21,6 +22,8 @@
 #include <asm/ppc_asm.h>
 #include <asm/asm-offsets.h>
 
+#undef DEBUG
+
 	.text
 
 /*
@@ -84,6 +87,19 @@ END_FTR_SECTION_IFSET(CPU_FTR_CAN_NAP)
 	cmpwi	0,r3,0
 	beqlr
 
+	/* Clear MSR:EE */
+	mfmsr	r7
+	rlwinm	r0,r7,0,17,15
+	mtmsr	r0
+
+	/* Check current_thread_info()->flags */
+	rlwinm	r4,r1,0,0,18
+	lwz	r4,TI_FLAGS(r4)
+	andi.	r0,r4,_TIF_NEED_RESCHED
+	beq	1f
+	mtmsr	r7	/* out of line this ? */
+	blr
+1:	
 	/* Some pre-nap cleanups needed on some CPUs */
 	andis.	r0,r3,HID0_NAP@h
 	beq	2f
@@ -106,6 +122,12 @@ BEGIN_FTR_SECTION
 	dcbf	0,r4
 	dcbf	0,r4
 END_FTR_SECTION_IFSET(CPU_FTR_NAP_DISABLE_L2_PR)
+#ifdef DEBUG
+	lis	r6,nap_enter_count@ha
+	lwz	r4,nap_enter_count@l(r6)
+	addi	r4,r4,1
+	stw	r4,nap_enter_count@l(r6)
+#endif	
 2:
 BEGIN_FTR_SECTION
 	/* Go to low speed mode on some 750FX */
@@ -135,42 +157,47 @@ BEGIN_FTR_SECTION
 	DSSALL
 	sync
 END_FTR_SECTION_IFSET(CPU_FTR_ALTIVEC)
-	rlwinm	r9,r1,0,0,31-THREAD_SHIFT	/* current thread_info */
-	lwz	r8,TI_LOCAL_FLAGS(r9)	/* set napping bit */
-	ori	r8,r8,_TLF_NAPPING	/* so when we take an exception */
-	stw	r8,TI_LOCAL_FLAGS(r9)	/* it will return to our caller */
-	mfmsr	r7
-	ori	r7,r7,MSR_EE
+	ori	r7,r7,MSR_EE /* Could be ommited (already set) */
 	oris	r7,r7,MSR_POW@h
-1:	sync
+	sync
+	isync
 	mtmsr	r7
 	isync
-	b	1b
-
+	sync
+	blr
+	
 /*
  * Return from NAP/DOZE mode, restore some CPU specific registers,
  * we are called with DR/IR still off and r2 containing physical
- * address of current.  R11 points to the exception frame (physical
- * address).  We have to preserve r10.
+ * address of current.
  */
 _GLOBAL(power_save_6xx_restore)
-	lwz	r9,_LINK(r11)		/* interrupted in ppc6xx_idle: */
-	stw	r9,_NIP(r11)		/* make it do a blr */
+	mfspr	r11,SPRN_HID0
+	rlwinm.	r11,r11,0,10,8	/* Clear NAP & copy NAP bit !state to cr1 EQ */
+	cror	4*cr1+eq,4*cr0+eq,4*cr0+eq
+BEGIN_FTR_SECTION
+	rlwinm	r11,r11,0,9,7	/* Clear DOZE */
+END_FTR_SECTION_IFSET(CPU_FTR_CAN_DOZE)
+	mtspr	SPRN_HID0, r11
 
-#ifdef CONFIG_SMP
-	mfspr	r12,SPRN_SPRG3
-	lwz	r11,TI_CPU(r12)		/* get cpu number * 4 */
-	slwi	r11,r11,2
-#else
-	li	r11,0
+#ifdef DEBUG
+	beq	cr1,1f
+	lis	r11,(nap_return_count-KERNELBASE)@ha
+	lwz	r9,nap_return_count@l(r11)
+	addi	r9,r9,1
+	stw	r9,nap_return_count@l(r11)
+1:
 #endif
+	
+	rlwinm	r9,r1,0,0,18
+	tophys(r9,r9)
+	lwz	r11,TI_CPU(r9)
+	slwi	r11,r11,2
 	/* Todo make sure all these are in the same page
-	 * and load r11 (@ha part + CPU offset) only once
+	 * and load r22 (@ha part + CPU offset) only once
 	 */
 BEGIN_FTR_SECTION
-	mfspr	r9,SPRN_HID0
-	andis.	r9,r9,HID0_NAP@h
-	beq	1f
+	beq	cr1,1f
 	addis	r9,r11,(nap_save_msscr0-KERNELBASE)@ha
 	lwz	r9,nap_save_msscr0@l(r9)
 	mtspr	SPRN_MSSCR0, r9
@@ -193,5 +220,14 @@ _GLOBAL(nap_save_msscr0)
 _GLOBAL(nap_save_hid1)
 	.space	4*NR_CPUS
 
+_GLOBAL(powersave_nap)
+	.long	0
 _GLOBAL(powersave_lowspeed)
 	.long	0
+
+#ifdef DEBUG
+_GLOBAL(nap_enter_count)
+	.space	4
+_GLOBAL(nap_return_count)
+	.space	4
+#endif