ENGR12345678- MX6: Bypass PLL1 during WAIT

    When system is going to enter WAIT mode, set PLL1 to 24MHz
    so that ARM is running at 24MHz.

Signed-off-by: Ranjani Vaidyanathan <ra5478@freescale.com>

3 files changed, 84 insertions, 30 deletions

diff --git a/arch/arm/mach-mx6/cpu.c b/arch/arm/mach-mx6/cpu.c
index 7b7668cf0a6a..279bed8e8c71 100644
--- a/arch/arm/mach-mx6/cpu.c
+++ b/arch/arm/mach-mx6/cpu.c
@@ -31,6 +31,7 @@
 #include "crm_regs.h"
 #include "cpu_op-mx6.h"
 
+extern unsigned int num_cpu_idle_lock;
 
 void *mx6_wait_in_iram_base;
 void (*mx6_wait_in_iram)(void);
@@ -128,26 +129,11 @@ static int __init post_cpu_init(void)
 	reg &= ~0x1;
 	__raw_writel(reg, base);
 
-	/* Allocate IRAM for WAIT code. */
-	/* Move wait routine into iRAM */
-	cpaddr = (unsigned long)iram_alloc(SZ_4K, &iram_paddr);
-	/* Need to remap the area here since we want the memory region
-		 to be executable. */
-	mx6_wait_in_iram_base = __arm_ioremap(iram_paddr, SZ_4K,
-					  MT_MEMORY_NONCACHED);
-	pr_info("cpaddr = %x wait_iram_base=%x\n",
-		(unsigned int)cpaddr, (unsigned int)mx6_wait_in_iram_base);
-
-	/*
-	 * Need to run the suspend code from IRAM as the DDR needs
-	 * to be put into low power mode manually.
-	 */
-	memcpy((void *)cpaddr, mx6_wait, SZ_4K);
-	mx6_wait_in_iram = (void *)mx6_wait_in_iram_base;
-
 	gpc_base = MX6_IO_ADDRESS(GPC_BASE_ADDR);
 	ccm_base = MX6_IO_ADDRESS(CCM_BASE_ADDR);
 
+	num_cpu_idle_lock = 0x0;
+
 	return 0;
 }
 postcore_initcall(post_cpu_init);
diff --git a/arch/arm/mach-mx6/mx6_wfi.S b/arch/arm/mach-mx6/mx6_wfi.S
index 3c6310263b38..6e2972faa293 100644
--- a/arch/arm/mach-mx6/mx6_wfi.S
+++ b/arch/arm/mach-mx6/mx6_wfi.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -17,6 +17,7 @@
  */
 
 #include <linux/linkage.h>
+#include <mach/hardware.h>
 
 /*
  *  mx6_wait
@@ -27,14 +28,80 @@
  */
 ENTRY(mx6_wait)
 
+    push {r4, r5, r6}
+
+      ldr     r2, =ANATOP_BASE_ADDR
+      add   r2, r2, #PERIPBASE_VIRT
+
+    /* get the flags variables into the cache */
+	ldr     r3, [r0]
+
+    /* get CPU ID */
+    mrc     p15,0,r5,c0,c0,5
+    and     r5, r5, #0x3
+
+    mov r4,#0xff
+    strb r4,[r0,r5]
+
+    dsb
+
+    mvn r4, #0x0
+    ldr r3, [r0]
+    cmp r3, r4
+    bne DO_WFI
+
+    mov r4, #0x1
+    ldrex r3, [r1]
+    cmp r3, #0x0
+    strexeq r3, r4, [r1]
+    cmpeq r3, #0x0
+    bne DO_WFI
+
+    mov r3, #0xff
+
+    ldr     r6, =(1 << 16)
+    str     r6, [r2, #0x04]
+
+    /* dmb */
+
+    str r3, [r1]
+
+    dsb
+
+    mvn r4, #0x0
+    ldr r3, [r0]
+    cmp r3, r4
+    movne r3, #0x0
+    strne r6, [r2, #0x08]
+    strne r3, [r1]
+
+DO_WFI:
 	dsb
 
 	wfi
 
- 	isb
-	isb
+    mov r4, #0x0
+    strb r4, [r0, r5]
+
+    dsb
+
+    ldr r3, [r1]
+    cmp r3, #0xff
+    bne DONE
+
+    mov r4, #0x0
+    ldr     r6, =(1 << 16)
+    str r6, [r2, #0x08]
+
+    mov r3, #0x0
+    str r3, [r1]
+
+DONE:
+
+    pop {r4,r5, r6}
 
-      mov     pc, lr
+    /* Restore registers */
+    mov     pc, lr
 
     .type   mx6_do_wait, #object
 ENTRY(mx6_do_wait)
diff --git a/arch/arm/mach-mx6/system.c b/arch/arm/mach-mx6/system.c
index 185be00db482..4702168599b2 100644
--- a/arch/arm/mach-mx6/system.c
+++ b/arch/arm/mach-mx6/system.c
@@ -47,11 +47,16 @@
 extern unsigned int gpc_wake_irq[4];
 extern int mx6q_revision(void);
 
+static unsigned int cpu_idle_mask;
+
 static void __iomem *gpc_base = IO_ADDRESS(GPC_BASE_ADDR);
 
-extern void (*mx6_wait_in_iram)(void);
-extern void mx6_wait(void);
-extern void *mx6_wait_in_iram_base;
+volatile unsigned int num_cpu_idle;
+volatile unsigned int num_cpu_idle_lock = 0x0;
+
+
+extern void (*mx6_wait_in_iram)(void *ccm_base);
+extern void mx6_wait(void *num_cpu_idle_lock, void *num_cpu_idle);
 extern bool enable_wait_mode;
 
 void gpc_set_wakeup(unsigned int irq[4])
@@ -156,13 +161,9 @@ void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
  void arch_idle(void)
 {
 	if (enable_wait_mode) {
-		if ((num_online_cpus() == num_present_cpus())
-			&& mx6_wait_in_iram != NULL) {
+		if (num_online_cpus() == num_present_cpus()) {
 			mxc_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
-			if (smp_processor_id() == 0)
-				mx6_wait_in_iram();
-			else
-				cpu_do_idle();
+			mx6_wait(&num_cpu_idle_lock, &num_cpu_idle);
 		}
 	} else
 		cpu_do_idle();