[PATCH v2 0/4] Support imx6q WAIT mode

[PATCH v2 0/4] Support imx6q WAIT mode

[Shawn Guo]

With more intensive testing on different revisions of imx6q, we found
that the power gating state can not always work reliably.  So v2 of
the series drops the support for that state and adds clock gating state
only.  As the result, we need not to use coupled cpuidle now.

Shawn Guo (4):
  ARM: imx: allow timer counter to roll over
  ARM: imx: mask gpc interrupts initially
  ARM: imx: move imx6q_cpuidle_driver into a separate file
  ARM: imx6q: support WAIT mode using cpuidle

 arch/arm/mach-imx/Makefile        |    6 ++-
 arch/arm/mach-imx/clk-imx6q.c     |   12 +++++
 arch/arm/mach-imx/common.h        |    3 ++
 arch/arm/mach-imx/cpuidle-imx6q.c |   98 +++++++++++++++++++++++++++++++++++++
 arch/arm/mach-imx/cpuidle.h       |    5 ++
 arch/arm/mach-imx/gpc.c           |    5 ++
 arch/arm/mach-imx/mach-imx6q.c    |   17 +++----
 arch/arm/mach-imx/platsmp.c       |   10 ++++
 arch/arm/mach-imx/time.c          |    6 +--
 9 files changed, 146 insertions(+), 16 deletions(-)
 create mode 100644 arch/arm/mach-imx/cpuidle-imx6q.c

-- 
1.7.9.5

[PATCH v2 1/4] ARM: imx: allow timer counter to roll over

[Shawn Guo]

The timer is configured in free-run mode.  The counter should be
allowed to roll over to 0 when reaching 0xffffffff.  Let's do that
by always returning 0 in set_next_event.

Signed-off-by: Shawn Guo <shawn.guo@linaro.org>
---
 arch/arm/mach-imx/time.c |    6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
index f017302..858098c 100644
--- a/arch/arm/mach-imx/time.c
+++ b/arch/arm/mach-imx/time.c
@@ -139,8 +139,7 @@ static int mx1_2_set_next_event(unsigned long evt,
 
 	__raw_writel(tcmp, timer_base + MX1_2_TCMP);
 
-	return (int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)) < 0 ?
-				-ETIME : 0;
+	return 0;
 }
 
 static int v2_set_next_event(unsigned long evt,
@@ -152,8 +151,7 @@ static int v2_set_next_event(unsigned long evt,
 
 	__raw_writel(tcmp, timer_base + V2_TCMP);
 
-	return (int)(tcmp - __raw_readl(timer_base + V2_TCN)) < 0 ?
-				-ETIME : 0;
+	return 0;
 }
 
 #ifdef DEBUG
-- 
1.7.9.5

[PATCH v2 1/4] ARM: imx: allow timer counter to roll over

[Russell King - ARM Linux]

On Tue, Dec 04, 2012 at 10:55:12PM +0800, Shawn Guo wrote:
> The timer is configured in free-run mode.  The counter should be
> allowed to roll over to 0 when reaching 0xffffffff.  Let's do that
> by always returning 0 in set_next_event.

Are you sure this is correct?  It looks wrong to me.

If the time set by the next event has passed, you must return -ETIME
from set_next_event() so that the generic timer code can compensate
for the missed event and recalculate it, otherwise you will end up
having to wait a full count cycle before receiving the next event.

If you find that you're being passed such a small increment that you're
constantly hitting that condition, you need to increase your minimum
waited interval.


> diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
> index f017302..858098c 100644
> --- a/arch/arm/mach-imx/time.c
> +++ b/arch/arm/mach-imx/time.c
> @@ -139,8 +139,7 @@ static int mx1_2_set_next_event(unsigned long evt,
>  
>  	__raw_writel(tcmp, timer_base + MX1_2_TCMP);
>  
> -	return (int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)) < 0 ?
> -				-ETIME : 0;
> +	return 0;

So what this code was doing is: tcmp is the counter value we expect with
the expected event time added.  This may wrap 32-bits.  We subtract the
current timer value after we've set the compare register.  If the current
timer value was larger than the expected event time, we return -ETIME.

That to me sounds 100% correct.  Your replacement code of always returning
zero looks wrong.

[PATCH v2 1/4] ARM: imx: allow timer counter to roll over

[Shawn Guo]

On Tue, Dec 04, 2012 at 03:44:59PM +0000, Russell King - ARM Linux wrote:
> On Tue, Dec 04, 2012 at 10:55:12PM +0800, Shawn Guo wrote:
> > The timer is configured in free-run mode.  The counter should be
> > allowed to roll over to 0 when reaching 0xffffffff.  Let's do that
> > by always returning 0 in set_next_event.
> 
> Are you sure this is correct?  It looks wrong to me.
> 
> If the time set by the next event has passed, you must return -ETIME
> from set_next_event() so that the generic timer code can compensate
> for the missed event and recalculate it, otherwise you will end up
> having to wait a full count cycle before receiving the next event.
> 
> If you find that you're being passed such a small increment that you're
> constantly hitting that condition, you need to increase your minimum
> waited interval.

I think we already have a proper min_delta_tick setting to ensure we
will not run into the situation that the event will get expired in
v2_set_next_event().  I guess that's the reason why most of the
set_next_event() implementations under arch/arm can always return 0?

> > diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
> > index f017302..858098c 100644
> > --- a/arch/arm/mach-imx/time.c
> > +++ b/arch/arm/mach-imx/time.c
> > @@ -139,8 +139,7 @@ static int mx1_2_set_next_event(unsigned long evt,
> >  
> >  	__raw_writel(tcmp, timer_base + MX1_2_TCMP);
> >  
> > -	return (int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)) < 0 ?
> > -				-ETIME : 0;
> > +	return 0;
> 
> So what this code was doing is: tcmp is the counter value we expect with
> the expected event time added.  This may wrap 32-bits.  We subtract the
> current timer value after we've set the compare register.  If the current
> timer value was larger than the expected event time, we return -ETIME.

>From what I have seen, it never happens in event expiring case but
always in counter wrapping.  Since the timer is configured in free-run
mode and can keep running after wrapping, it should be safe to always
return zero.

> That to me sounds 100% correct.  Your replacement code of always returning
> zero looks wrong.

The code change is fixing a problem seen with WAIT mode support, without
introducing any regression from what I've seen.

Enabling WAIT mode support with the original code, we will easily run
into an infinite loop in tick_handle_oneshot_broadcast() - "goto again;"
This happens because when global event did not expire any CPU local
events, the broadcast device will be rearmed to a CPU local next_event,
which could be far away from now and result in a max_delta_tick
programming in set_next_event().  This will certainly cause a counter
wrapping and then a -ETIME return, which in turn makes the execution
fall into the "goto again" loop.

One thing I do not understand is why tick_broadcast_set_event() is being
called with parameter "force" being 0 in tick_handle_oneshot_broadcast().
This will make clockevents_program_event() skip
clockevents_program_min_delta() call in that case.  Changing "force"
to 1 also fixes my problem, but I wouldn't be so aggressive to change
the clockevents core code when there is a fix that could be applied
at platform level.

Shawn

[PATCH v2 1/4] ARM: imx: allow timer counter to roll over

[Russell King - ARM Linux]

On Wed, Dec 05, 2012 at 08:14:51PM +0800, Shawn Guo wrote:
> On Tue, Dec 04, 2012 at 03:44:59PM +0000, Russell King - ARM Linux wrote:
> > On Tue, Dec 04, 2012 at 10:55:12PM +0800, Shawn Guo wrote:
> > > The timer is configured in free-run mode.  The counter should be
> > > allowed to roll over to 0 when reaching 0xffffffff.  Let's do that
> > > by always returning 0 in set_next_event.
> > 
> > Are you sure this is correct?  It looks wrong to me.
> > 
> > If the time set by the next event has passed, you must return -ETIME
> > from set_next_event() so that the generic timer code can compensate
> > for the missed event and recalculate it, otherwise you will end up
> > having to wait a full count cycle before receiving the next event.
> > 
> > If you find that you're being passed such a small increment that you're
> > constantly hitting that condition, you need to increase your minimum
> > waited interval.
> 
> I think we already have a proper min_delta_tick setting to ensure we
> will not run into the situation that the event will get expired in
> v2_set_next_event().  I guess that's the reason why most of the
> set_next_event() implementations under arch/arm can always return 0?

There are two cases here: there are those where there is a free-running
counter which continually increments/decrements, and it is compared to
a match value to trigger the next interrupt.

There are also those cases where you load the timer with the count value
and it counts towards the interrupt trigger value (either up or down).

With the former, you should check that the event has not passed before
returning, and if it has, you must return -ETIME, particularly if the
wrap takes several seconds.  With the latter, you're going to time out
reasonably close to the desired time anyway, so there's no need to do
any checking (you can't in any case.)  So in this case, you would always
return zero.

> > > diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
> > > index f017302..858098c 100644
> > > --- a/arch/arm/mach-imx/time.c
> > > +++ b/arch/arm/mach-imx/time.c
> > > @@ -139,8 +139,7 @@ static int mx1_2_set_next_event(unsigned long evt,
> > >  
> > >  	__raw_writel(tcmp, timer_base + MX1_2_TCMP);
> > >  
> > > -	return (int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)) < 0 ?
> > > -				-ETIME : 0;
> > > +	return 0;
> > 
> > So what this code was doing is: tcmp is the counter value we expect with
> > the expected event time added.  This may wrap 32-bits.  We subtract the
> > current timer value after we've set the compare register.  If the current
> > timer value was larger than the expected event time, we return -ETIME.
> 
> >From what I have seen, it never happens in event expiring case but
> always in counter wrapping.  Since the timer is configured in free-run
> mode and can keep running after wrapping, it should be safe to always
> return zero.

Well, let's look at the maths here.

Let's say that the counter value is at 0xfffffff0, and the desired timeout
is 32.  This makes tcmp 0x00000010.  Let's say that the counter has only
incremented a small amount to 0xfffffff8:

(int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)

So this becomes 0x10 - 0xfffffff8, which will be 0x18.  So the return value
in this case will be zero.

Now lets consider what happens if the timer has wrapped to 0x00000008.  The
calculation becomes 0x10 - 0x08, which is 0x08.  Again, this is still
positive, so the resulting return value will still be zero.

Now for the boundary condition.  Lets say the timer has incremented to 0x10.
0x10 - 0x10 is 0x00, which is still positive, so the return value will be
zero.  But the next count, 0x11 produces a difference.  0x10 - 0x11 is
negative, so the return value will be -ETIME.

It appears that the original code here is doing the right thing: it's
returning -ETIME if the event which has been programmed has already passed.

> > That to me sounds 100% correct.  Your replacement code of always returning
> > zero looks wrong.
> 
> The code change is fixing a problem seen with WAIT mode support, without
> introducing any regression from what I've seen.
> 
> Enabling WAIT mode support with the original code, we will easily run
> into an infinite loop in tick_handle_oneshot_broadcast() - "goto again;"
> This happens because when global event did not expire any CPU local
> events, the broadcast device will be rearmed to a CPU local next_event,
> which could be far away from now and result in a max_delta_tick
> programming in set_next_event().

Ah, so it's not a wrap of the counter caused by too small an increment,
but a wrap of the timeout value causing the timeout value to potentially
appear wrapped.  That's a slightly more complex case to deal with; how
sa11x0 and PXA deals with this by restricting the max_delta_tick to avoid
the problem:

        ckevt_sa1100_osmr0.max_delta_ns =
                clockevent_delta2ns(0x7fffffff, &ckevt_sa1100_osmr0);
        ckevt_pxa_osmr0.max_delta_ns =
                clockevent_delta2ns(0x7fffffff, &ckevt_pxa_osmr0);

Another solution which avoids restricting max_delta_tick would be to
detect the large increment and avoid the check in that case, so:

	return evt < ((u32)~0) / 2 &&
		(int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)) < 0 ?
				-ETIME : 0;

> One thing I do not understand is why tick_broadcast_set_event() is being
> called with parameter "force" being 0 in tick_handle_oneshot_broadcast().

Presumably because the design of the code is to detect whenever the
requested event has passed, and recalculate the expiry time, rather
than just trying to set a timeout that will occur immediately.

[PATCH v2 1/4] ARM: imx: allow timer counter to roll over

[Shawn Guo]

On Wed, Dec 05, 2012 at 12:09:00PM +0000, Russell King - ARM Linux wrote:
> Well, let's look at the maths here.
> 
> Let's say that the counter value is at 0xfffffff0, and the desired timeout
> is 32.  This makes tcmp 0x00000010.  Let's say that the counter has only
> incremented a small amount to 0xfffffff8:
> 
> (int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)
> 
> So this becomes 0x10 - 0xfffffff8, which will be 0x18.  So the return value
> in this case will be zero.
> 
> Now lets consider what happens if the timer has wrapped to 0x00000008.  The
> calculation becomes 0x10 - 0x08, which is 0x08.  Again, this is still
> positive, so the resulting return value will still be zero.
> 
> Now for the boundary condition.  Lets say the timer has incremented to 0x10.
> 0x10 - 0x10 is 0x00, which is still positive, so the return value will be
> zero.  But the next count, 0x11 produces a difference.  0x10 - 0x11 is
> negative, so the return value will be -ETIME.
> 
> It appears that the original code here is doing the right thing: it's
> returning -ETIME if the event which has been programmed has already passed.

This is a helpful explanation.  I was indeed fix my problem in a wrong
way.

> > > That to me sounds 100% correct.  Your replacement code of always returning
> > > zero looks wrong.
> > 
> > The code change is fixing a problem seen with WAIT mode support, without
> > introducing any regression from what I've seen.
> > 
> > Enabling WAIT mode support with the original code, we will easily run
> > into an infinite loop in tick_handle_oneshot_broadcast() - "goto again;"
> > This happens because when global event did not expire any CPU local
> > events, the broadcast device will be rearmed to a CPU local next_event,
> > which could be far away from now and result in a max_delta_tick
> > programming in set_next_event().
> 
> Ah, so it's not a wrap of the counter caused by too small an increment,
> but a wrap of the timeout value causing the timeout value to potentially
> appear wrapped.  That's a slightly more complex case to deal with; how
> sa11x0 and PXA deals with this by restricting the max_delta_tick to avoid
> the problem:
> 
>         ckevt_sa1100_osmr0.max_delta_ns =
>                 clockevent_delta2ns(0x7fffffff, &ckevt_sa1100_osmr0);
>         ckevt_pxa_osmr0.max_delta_ns =
>                 clockevent_delta2ns(0x7fffffff, &ckevt_pxa_osmr0);
> 
> Another solution which avoids restricting max_delta_tick would be to
> detect the large increment and avoid the check in that case, so:
> 
> 	return evt < ((u32)~0) / 2 &&
> 		(int)(tcmp - __raw_readl(timer_base + MX1_2_TCN)) < 0 ?
> 				-ETIME : 0;
> 
I will go for this one, since it keeps max_delta_tick stay as big as
possible.

> > One thing I do not understand is why tick_broadcast_set_event() is being
> > called with parameter "force" being 0 in tick_handle_oneshot_broadcast().
> 
> Presumably because the design of the code is to detect whenever the
> requested event has passed, and recalculate the expiry time, rather
> than just trying to set a timeout that will occur immediately.

Thanks for help understand all these, Russell.

Shawn

[PATCH v3 1/4] ARM: imx: return zero in case next event gets a large increment

[Shawn Guo]

The return of v2_set_next_event() will lead to an infinite loop in
tick_handle_oneshot_broadcast() - "goto again;" with imx6q WAIT mode
(to be enabled).  This happens because when global event did not expire
any CPU local events, the broadcast device will be rearmed to a CPU
local next_event, which could be far away from now and result in a
max_delta_tick programming in set_next_event().

Fix the problem by detecting those next events with increments larger
than 0x7fffffff, and simply return zero in that case.

It leaves mx1_2_set_next_event() unchanged since only v2_set_next_event()
will be running with imx6q WAIT mode support.

Thanks Russell King for helping understand the problem.

Signed-off-by: Shawn Guo <shawn.guo@linaro.org>
---
 arch/arm/mach-imx/time.c |    3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
index f017302..5d7eb48 100644
--- a/arch/arm/mach-imx/time.c
+++ b/arch/arm/mach-imx/time.c
@@ -152,7 +152,8 @@ static int v2_set_next_event(unsigned long evt,
 
 	__raw_writel(tcmp, timer_base + V2_TCMP);
 
-	return (int)(tcmp - __raw_readl(timer_base + V2_TCN)) < 0 ?
+	return evt < 0x7fffffff &&
+		(int)(tcmp - __raw_readl(timer_base + V2_TCN)) < 0 ?
 				-ETIME : 0;
 }
 
-- 
1.7.9.5

[PATCH v2 2/4] ARM: imx: mask gpc interrupts initially

[Shawn Guo]

Mask gpc interrupts initially to avoid suspicious interrupts.

Signed-off-by: Shawn Guo <shawn.guo@linaro.org>
---
 arch/arm/mach-imx/gpc.c |    5 +++++
 1 file changed, 5 insertions(+)

diff --git a/arch/arm/mach-imx/gpc.c b/arch/arm/mach-imx/gpc.c
index e1537f9..722e5df 100644
--- a/arch/arm/mach-imx/gpc.c
+++ b/arch/arm/mach-imx/gpc.c
@@ -101,11 +101,16 @@ static void imx_gpc_irq_mask(struct irq_data *d)
 void __init imx_gpc_init(void)
 {
 	struct device_node *np;
+	int i;
 
 	np = of_find_compatible_node(NULL, NULL, "fsl,imx6q-gpc");
 	gpc_base = of_iomap(np, 0);
 	WARN_ON(!gpc_base);
 
+	/* Initially mask all interrupts */
+	for (i = 0; i < IMR_NUM; i++)
+		writel_relaxed(~0, gpc_base + GPC_IMR1 + i * 4);
+
 	/* Register GPC as the secondary interrupt controller behind GIC */
 	gic_arch_extn.irq_mask = imx_gpc_irq_mask;
 	gic_arch_extn.irq_unmask = imx_gpc_irq_unmask;
-- 
1.7.9.5

[PATCH v2 3/4] ARM: imx: move imx6q_cpuidle_driver into a separate file

[Shawn Guo]

Move imx6q_cpuidle_driver into a separate file as more codes will
be added when WAIT mode gets implemented as cpuidle.

Signed-off-by: Shawn Guo <shawn.guo@linaro.org>
---
 arch/arm/mach-imx/Makefile        |    6 +++++-
 arch/arm/mach-imx/cpuidle-imx6q.c |   26 ++++++++++++++++++++++++++
 arch/arm/mach-imx/cpuidle.h       |    5 +++++
 arch/arm/mach-imx/mach-imx6q.c    |   12 +-----------
 4 files changed, 37 insertions(+), 12 deletions(-)
 create mode 100644 arch/arm/mach-imx/cpuidle-imx6q.c

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 0634b31..49799fd 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -28,7 +28,11 @@ obj-$(CONFIG_MXC_ULPI) += ulpi.o
 obj-$(CONFIG_MXC_USE_EPIT) += epit.o
 obj-$(CONFIG_MXC_DEBUG_BOARD) += 3ds_debugboard.o
 obj-$(CONFIG_CPU_FREQ_IMX)    += cpufreq.o
-obj-$(CONFIG_CPU_IDLE) += cpuidle.o
+
+ifeq ($(CONFIG_CPU_IDLE),y)
+obj-y += cpuidle.o
+obj-$(CONFIG_SOC_IMX6Q) += cpuidle-imx6q.o
+endif
 
 ifdef CONFIG_SND_IMX_SOC
 obj-y += ssi-fiq.o
diff --git a/arch/arm/mach-imx/cpuidle-imx6q.c b/arch/arm/mach-imx/cpuidle-imx6q.c
new file mode 100644
index 0000000..83facc9
--- /dev/null
+++ b/arch/arm/mach-imx/cpuidle-imx6q.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/cpuidle.h>
+#include <linux/module.h>
+#include <asm/cpuidle.h>
+
+#include "cpuidle.h"
+
+static struct cpuidle_driver imx6q_cpuidle_driver = {
+	.name = "imx6q_cpuidle",
+	.owner = THIS_MODULE,
+	.en_core_tk_irqen = 1,
+	.states[0] = ARM_CPUIDLE_WFI_STATE,
+	.state_count = 1,
+};
+
+int __init imx6q_cpuidle_init(void)
+{
+	return imx_cpuidle_init(&imx6q_cpuidle_driver);
+}
diff --git a/arch/arm/mach-imx/cpuidle.h b/arch/arm/mach-imx/cpuidle.h
index bc932d1..e092d13 100644
--- a/arch/arm/mach-imx/cpuidle.h
+++ b/arch/arm/mach-imx/cpuidle.h
@@ -14,9 +14,14 @@
 
 #ifdef CONFIG_CPU_IDLE
 extern int imx_cpuidle_init(struct cpuidle_driver *drv);
+extern int imx6q_cpuidle_init(void);
 #else
 static inline int imx_cpuidle_init(struct cpuidle_driver *drv)
 {
 	return -ENODEV;
 }
+static inline int imx6q_cpuidle_init(void)
+{
+	return -ENODEV;
+}
 #endif
diff --git a/arch/arm/mach-imx/mach-imx6q.c b/arch/arm/mach-imx/mach-imx6q.c
index 4eb1b3a..4ea7ad9 100644
--- a/arch/arm/mach-imx/mach-imx6q.c
+++ b/arch/arm/mach-imx/mach-imx6q.c
@@ -12,7 +12,6 @@
 
 #include <linux/clk.h>
 #include <linux/clkdev.h>
-#include <linux/cpuidle.h>
 #include <linux/delay.h>
 #include <linux/export.h>
 #include <linux/init.h>
@@ -26,7 +25,6 @@
 #include <linux/regmap.h>
 #include <linux/micrel_phy.h>
 #include <linux/mfd/syscon.h>
-#include <asm/cpuidle.h>
 #include <asm/smp_twd.h>
 #include <asm/hardware/cache-l2x0.h>
 #include <asm/hardware/gic.h>
@@ -201,17 +199,9 @@ static void __init imx6q_init_machine(void)
 	imx6q_1588_init();
 }
 
-static struct cpuidle_driver imx6q_cpuidle_driver = {
-	.name			= "imx6q_cpuidle",
-	.owner			= THIS_MODULE,
-	.en_core_tk_irqen	= 1,
-	.states[0]		= ARM_CPUIDLE_WFI_STATE,
-	.state_count		= 1,
-};
-
 static void __init imx6q_init_late(void)
 {
-	imx_cpuidle_init(&imx6q_cpuidle_driver);
+	imx6q_cpuidle_init();
 }
 
 static void __init imx6q_map_io(void)
-- 
1.7.9.5

[PATCH v2 4/4] ARM: imx6q: support WAIT mode using cpuidle

[Shawn Guo]

Add WAIT mode (ARM core clock gating) support to imx6q cpuidle driver.
As WAIT mode is broken on imx6q TO 1.0 and 1.1, it only enables the
support for revision 1.2 with chicken bit set.

Signed-off-by: Shawn Guo <shawn.guo@linaro.org>
---
 arch/arm/mach-imx/clk-imx6q.c     |   12 ++++++
 arch/arm/mach-imx/common.h        |    3 ++
 arch/arm/mach-imx/cpuidle-imx6q.c |   76 ++++++++++++++++++++++++++++++++++++-
 arch/arm/mach-imx/mach-imx6q.c    |    7 +++-
 arch/arm/mach-imx/platsmp.c       |   10 +++++
 5 files changed, 105 insertions(+), 3 deletions(-)

diff --git a/arch/arm/mach-imx/clk-imx6q.c b/arch/arm/mach-imx/clk-imx6q.c
index 7f2c10c..c714219 100644
--- a/arch/arm/mach-imx/clk-imx6q.c
+++ b/arch/arm/mach-imx/clk-imx6q.c
@@ -54,10 +54,21 @@
 #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)
 {
 	u32 val = readl_relaxed(ccm_base + CLPCR);
@@ -68,6 +79,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;
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 7191ab4..7edd53d 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -127,9 +127,11 @@ extern u32 *pl310_get_save_ptr(void);
 extern void v7_secondary_startup(void);
 extern void imx_scu_map_io(void);
 extern void imx_smp_prepare(void);
+extern void imx_scu_standby_enable(void);
 #else
 static inline void imx_scu_map_io(void) {}
 static inline void imx_smp_prepare(void) {}
+static inline void imx_scu_standby_enable(void) {}
 #endif
 extern void imx_enable_cpu(int cpu, bool enable);
 extern void imx_set_cpu_jump(int cpu, void *jump_addr);
@@ -139,6 +141,7 @@ extern void imx_gpc_init(void);
 extern void imx_gpc_pre_suspend(void);
 extern void imx_gpc_post_resume(void);
 extern int imx6q_set_lpm(enum mxc_cpu_pwr_mode mode);
+extern void imx6q_set_chicken_bit(void);
 extern void imx6q_clock_map_io(void);
 
 extern void imx_cpu_die(unsigned int cpu);
diff --git a/arch/arm/mach-imx/cpuidle-imx6q.c b/arch/arm/mach-imx/cpuidle-imx6q.c
index 83facc9..ddc26c2 100644
--- a/arch/arm/mach-imx/cpuidle-imx6q.c
+++ b/arch/arm/mach-imx/cpuidle-imx6q.c
@@ -6,21 +6,93 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/clockchips.h>
 #include <linux/cpuidle.h>
 #include <linux/module.h>
 #include <asm/cpuidle.h>
+#include <asm/proc-fns.h>
 
+#include "common.h"
 #include "cpuidle.h"
 
+static atomic_t master = ATOMIC_INIT(0);
+static DEFINE_SPINLOCK(master_lock);
+
+static int imx6q_enter_wait(struct cpuidle_device *dev,
+			    struct cpuidle_driver *drv, int index)
+{
+	int cpu = dev->cpu;
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
+
+	if (atomic_inc_return(&master) == num_online_cpus()) {
+		/*
+		 * With this lock, we prevent other cpu to exit and enter
+		 * this function again and become the master.
+		 */
+		if (!spin_trylock(&master_lock))
+			goto idle;
+		imx6q_set_lpm(WAIT_UNCLOCKED);
+		cpu_do_idle();
+		imx6q_set_lpm(WAIT_CLOCKED);
+		spin_unlock(&master_lock);
+		goto done;
+	}
+
+idle:
+	cpu_do_idle();
+done:
+	atomic_dec(&master);
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
+
+	return index;
+}
+
+/*
+ * For each cpu, setup the broadcast timer because local timer
+ * stops for the states other than WFI.
+ */
+static void imx6q_setup_broadcast_timer(void *arg)
+{
+	int cpu = smp_processor_id();
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ON, &cpu);
+}
+
 static struct cpuidle_driver imx6q_cpuidle_driver = {
 	.name = "imx6q_cpuidle",
 	.owner = THIS_MODULE,
 	.en_core_tk_irqen = 1,
-	.states[0] = ARM_CPUIDLE_WFI_STATE,
-	.state_count = 1,
+	.states = {
+		/* WFI */
+		ARM_CPUIDLE_WFI_STATE,
+		/* WAIT */
+		{
+			.exit_latency = 50,
+			.target_residency = 75,
+			.flags = CPUIDLE_FLAG_TIME_VALID,
+			.enter = imx6q_enter_wait,
+			.name = "WAIT",
+			.desc = "Clock off",
+		},
+	},
+	.state_count = 2,
+	.safe_state_index = 0,
 };
 
 int __init imx6q_cpuidle_init(void)
 {
+	/* Set initial power mode */
+	imx6q_set_lpm(WAIT_CLOCKED);
+
+	/* Need to enable SCU standby for entering WAIT modes */
+	imx_scu_standby_enable();
+
+	/* Set chicken bit to get a reliable WAIT mode support */
+	imx6q_set_chicken_bit();
+
+	/* Configure the broadcast timer on each cpu */
+	on_each_cpu(imx6q_setup_broadcast_timer, NULL, 1);
+
 	return imx_cpuidle_init(&imx6q_cpuidle_driver);
 }
diff --git a/arch/arm/mach-imx/mach-imx6q.c b/arch/arm/mach-imx/mach-imx6q.c
index 4ea7ad9..cda7622 100644
--- a/arch/arm/mach-imx/mach-imx6q.c
+++ b/arch/arm/mach-imx/mach-imx6q.c
@@ -201,7 +201,12 @@ static void __init imx6q_init_machine(void)
 
 static void __init imx6q_init_late(void)
 {
-	imx6q_cpuidle_init();
+	/*
+	 * WAIT mode is broken on TO 1.0 and 1.1, so there is no point
+	 * to run cpuidle on them.
+	 */
+	if (imx6q_revision() > IMX_CHIP_REVISION_1_1)
+		imx6q_cpuidle_init();
 }
 
 static void __init imx6q_map_io(void)
diff --git a/arch/arm/mach-imx/platsmp.c b/arch/arm/mach-imx/platsmp.c
index 3777b80..a70b548 100644
--- a/arch/arm/mach-imx/platsmp.c
+++ b/arch/arm/mach-imx/platsmp.c
@@ -20,6 +20,8 @@
 #include "common.h"
 #include "hardware.h"
 
+#define SCU_STANDBY_ENABLE	(1 << 5)
+
 static void __iomem *scu_base;
 
 static struct map_desc scu_io_desc __initdata = {
@@ -42,6 +44,14 @@ void __init imx_scu_map_io(void)
 	scu_base = IMX_IO_ADDRESS(base);
 }
 
+void imx_scu_standby_enable(void)
+{
+	u32 val = readl_relaxed(scu_base);
+
+	val |= SCU_STANDBY_ENABLE;
+	writel_relaxed(val, scu_base);
+}
+
 static void __cpuinit imx_secondary_init(unsigned int cpu)
 {
 	/*
-- 
1.7.9.5