OMAP2+: Fix clockdomain state programming

index ab87854..1de6def 100644
@@ -795,6 +795,27 @@ void clkdm_deny_idle(struct clockdomain *clkdm)
 	arch_clkdm->clkdm_deny_idle(clkdm);
 }
 
+/**
+ * clkdm_is_idle - Check if the clkdm hwsup/autoidle is enabled
+ * @clkdm: struct clockdomain *
+ *
+ * Returns true if the clockdomain is in hardware-supervised
+ * idle mode, or 0 otherwise.
+ *
+ */
+int clkdm_is_idle(struct clockdomain *clkdm)
+{
+	if (!clkdm)
+		return -EINVAL;
+
+	if (!arch_clkdm || !arch_clkdm->clkdm_is_idle)
+		return -EINVAL;
+
+	pr_debug("clockdomain: reading idle state for %s\n", clkdm->name);
+
+	return arch_clkdm->clkdm_is_idle(clkdm);
+}
+
 
 /* Clockdomain-to-clock framework interface code */
 

index 85b3dce..2fe3412 100644
@@ -138,6 +138,7 @@ struct clockdomain {
  * @clkdm_wakeup: Force a clockdomain to wakeup
  * @clkdm_allow_idle: Enable hw supervised idle transitions for clock domain
  * @clkdm_deny_idle: Disable hw supervised idle transitions for clock domain
+ * @clkdm_is_idle: Check if hw supervised idle transitions are enabled
  * @clkdm_clk_enable: Put the clkdm in right state for a clock enable
  * @clkdm_clk_disable: Put the clkdm in right state for a clock disable
  */

@@ -154,6 +155,7 @@ struct clkdm_ops {
 	int	(*clkdm_wakeup)(struct clockdomain *clkdm);
 	void	(*clkdm_allow_idle)(struct clockdomain *clkdm);
 	void	(*clkdm_deny_idle)(struct clockdomain *clkdm);
+	int	(*clkdm_is_idle)(struct clockdomain *clkdm);
 	int	(*clkdm_clk_enable)(struct clockdomain *clkdm);
 	int	(*clkdm_clk_disable)(struct clockdomain *clkdm);
 };

@@ -177,6 +179,7 @@ int clkdm_clear_all_sleepdeps(struct clockdomain *clkdm);
 
 void clkdm_allow_idle(struct clockdomain *clkdm);
 void clkdm_deny_idle(struct clockdomain *clkdm);
+int clkdm_is_idle(struct clockdomain *clkdm);
 
 int clkdm_wakeup(struct clockdomain *clkdm);
 int clkdm_sleep(struct clockdomain *clkdm);

index 48d0db7..3cdf95b 100644
@@ -13,6 +13,7 @@
  */
 
 #include <linux/types.h>
+#include <linux/errno.h>
 #include <plat/prcm.h>
 #include "prm.h"
 #include "prm2xxx_3xxx.h"

@@ -146,6 +147,15 @@ static void omap2_clkdm_deny_idle(struct clockdomain *clkdm)
 		_clkdm_del_autodeps(clkdm);
 }
 
+static int omap2_clkdm_is_idle(struct clockdomain *clkdm)
+{
+	if (!clkdm->clktrctrl_mask)
+		return -EINVAL;
+
+	return omap2_cm_is_clkdm_in_hwsup(clkdm->pwrdm.ptr->prcm_offs,
+				clkdm->clktrctrl_mask);
+}
+
 static void _enable_hwsup(struct clockdomain *clkdm)
 {
 	if (cpu_is_omap24xx())

@@ -252,6 +262,7 @@ struct clkdm_ops omap2_clkdm_operations = {
 	.clkdm_wakeup		= omap2_clkdm_wakeup,
 	.clkdm_allow_idle	= omap2_clkdm_allow_idle,
 	.clkdm_deny_idle	= omap2_clkdm_deny_idle,
+	.clkdm_is_idle		= omap2_clkdm_is_idle,
 	.clkdm_clk_enable	= omap2_clkdm_clk_enable,
 	.clkdm_clk_disable	= omap2_clkdm_clk_disable,
 };

@@ -269,6 +280,7 @@ struct clkdm_ops omap3_clkdm_operations = {
 	.clkdm_wakeup		= omap3_clkdm_wakeup,
 	.clkdm_allow_idle	= omap3_clkdm_allow_idle,
 	.clkdm_deny_idle	= omap3_clkdm_deny_idle,
+	.clkdm_is_idle		= omap2_clkdm_is_idle,
 	.clkdm_clk_enable	= omap2_clkdm_clk_enable,
 	.clkdm_clk_disable	= omap2_clkdm_clk_disable,
 };

index a1a4ecd..4b10727 100644
@@ -93,6 +93,12 @@ static void omap4_clkdm_deny_idle(struct clockdomain *clkdm)
 					clkdm->cm_inst, clkdm->clkdm_offs);
 }
 
+static int omap4_clkdm_is_idle(struct clockdomain *clkdm)
+{
+	return omap4_cminst_is_clkdm_in_hwsup(clkdm->prcm_partition,
+				clkdm->cm_inst, clkdm->clkdm_offs);
+}
+
 static int omap4_clkdm_clk_enable(struct clockdomain *clkdm)
 {
 	bool hwsup = false;

@@ -132,6 +138,7 @@ struct clkdm_ops omap4_clkdm_operations = {
 	.clkdm_wakeup		= omap4_clkdm_wakeup,
 	.clkdm_allow_idle	= omap4_clkdm_allow_idle,
 	.clkdm_deny_idle	= omap4_clkdm_deny_idle,
+	.clkdm_is_idle		= omap4_clkdm_is_idle,
 	.clkdm_clk_enable	= omap4_clkdm_clk_enable,
 	.clkdm_clk_disable	= omap4_clkdm_clk_disable,
 };

index 30af335..04b6da7 100644
@@ -105,7 +105,7 @@ static void omap2_init_processor_devices(void)
 int omap_set_pwrdm_state(struct powerdomain *pwrdm, u32 state)
 {
 	u32 cur_state;
-	int sleep_switch = 0;
+	int sleep_switch = -1;
 	int ret = 0;
 
 	if (pwrdm == NULL || IS_ERR(pwrdm))

index 04b6da7..b1190c4 100644
@@ -107,6 +107,7 @@ int omap_set_pwrdm_state(struct powerdomain *pwrdm, u32 state)
 	u32 cur_state;
 	int sleep_switch = -1;
 	int ret = 0;
+	int hwsup = 0;
 
 	if (pwrdm == NULL || IS_ERR(pwrdm))
 		return -EINVAL;

@@ -126,6 +127,7 @@ int omap_set_pwrdm_state(struct powerdomain *pwrdm, u32 state)
 			(pwrdm->flags & PWRDM_HAS_LOWPOWERSTATECHANGE)) {
 			sleep_switch = LOWPOWERSTATE_SWITCH;
 		} else {
+			hwsup = clkdm_is_idle(pwrdm->pwrdm_clkdms[0]);
 			clkdm_wakeup(pwrdm->pwrdm_clkdms[0]);
 			pwrdm_wait_transition(pwrdm);
 			sleep_switch = FORCEWAKEUP_SWITCH;

@@ -141,7 +143,7 @@ int omap_set_pwrdm_state(struct powerdomain *pwrdm, u32 state)
 
 	switch (sleep_switch) {
 	case FORCEWAKEUP_SWITCH:
-		if (pwrdm->pwrdm_clkdms[0]->flags & CLKDM_CAN_ENABLE_AUTO)
+		if (hwsup)
 			clkdm_allow_idle(pwrdm->pwrdm_clkdms[0]);
 		else
 			clkdm_sleep(pwrdm->pwrdm_clkdms[0]);