1 files changed, 14 insertions, 1 deletions

diff --git a/arch/arm/mach-imx/clk-imx6q.c b/arch/arm/mach-imx/clk-imx6q.c
index 7f2c10c7413a..540138c4606c 100644
--- a/arch/arm/mach-imx/clk-imx6q.c
+++ b/arch/arm/mach-imx/clk-imx6q.c
@@ -54,9 +54,18 @@
 #define BM_CLPCR_MASK_SCU_IDLE          (0x1 << 26)
 #define BM_CLPCR_MASK_L2CC_IDLE         (0x1 << 27)
 
+#define CGPR                            0x64
+#define BM_CGPR_CHICKEN_BIT             (0x1 << 17)
+
 static void __iomem *ccm_base;
 
-void __init imx6q_clock_map_io(void) { }
+void imx6q_set_chicken_bit(void)
+{
+        u32 val = readl_relaxed(ccm_base + CGPR);
+
+        val |= BM_CGPR_CHICKEN_BIT;
+        writel_relaxed(val, ccm_base + CGPR);
+}
 
 int imx6q_set_lpm(enum mxc_cpu_pwr_mode mode)
 {
@@ -68,6 +77,7 @@ int imx6q_set_lpm(enum mxc_cpu_pwr_mode mode)
                 break;
         case WAIT_UNCLOCKED:
                 val |= 0x1 << BP_CLPCR_LPM;
+                val |= BM_CLPCR_ARM_CLK_DIS_ON_LPM;
                 break;
         case STOP_POWER_ON:
                 val |= 0x2 << BP_CLPCR_LPM;
@@ -436,6 +446,9 @@ int __init mx6q_clocks_init(void)
         for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
                 clk_prepare_enable(clk[clks_init_on[i]]);
 
+        /* Set initial power mode */
+        imx6q_set_lpm(WAIT_CLOCKED);
+
         np = of_find_compatible_node(NULL, NULL, "fsl,imx6q-gpt");
         base = of_iomap(np, 0);
         WARN_ON(!base);