ARM: OMAP2+: hwmod: Fix SOFTRESET logic

[pandora-kernel.git] / arch / arm / mach-omap2 / omap_hwmod.c

diff --git a/arch/arm/mach-omap2/omap_hwmod.c b/arch/arm/mach-omap2/omap_hwmod.c
index 6b3088d..17ccdfa 100644 (file)
--- a/arch/arm/mach-omap2/omap_hwmod.c
+++ b/arch/arm/mach-omap2/omap_hwmod.c
@@ -316,7 +316,7 @@ static int _set_clockactivity(struct omap_hwmod *oh, u8 clockact, u32 *v)
 }
 
 /**
- * _set_softreset: set OCP_SYSCONFIG.CLOCKACTIVITY bits in @v
+ * _set_softreset: set OCP_SYSCONFIG.SOFTRESET bit in @v
  * @oh: struct omap_hwmod *
  * @v: pointer to register contents to modify
  *
@@ -343,6 +343,36 @@ static int _set_softreset(struct omap_hwmod *oh, u32 *v)
        return 0;
 }
 
+/**
+ * _clear_softreset: clear OCP_SYSCONFIG.SOFTRESET bit in @v
+ * @oh: struct omap_hwmod *
+ * @v: pointer to register contents to modify
+ *
+ * Clear the SOFTRESET bit in @v for hwmod @oh.  Returns -EINVAL upon
+ * error or 0 upon success.
+ */
+static int _clear_softreset(struct omap_hwmod *oh, u32 *v)
+{
+       u32 softrst_mask;
+
+       if (!oh->class->sysc ||
+           !(oh->class->sysc->sysc_flags & SYSC_HAS_SOFTRESET))
+               return -EINVAL;
+
+       if (!oh->class->sysc->sysc_fields) {
+               WARN(1,
+                    "omap_hwmod: %s: sysc_fields absent for sysconfig class\n",
+                    oh->name);
+               return -EINVAL;
+       }
+
+       softrst_mask = (0x1 << oh->class->sysc->sysc_fields->srst_shift);
+
+       *v &= ~softrst_mask;
+
+       return 0;
+}
+
 /**
  * _set_module_autoidle: set the OCP_SYSCONFIG AUTOIDLE field in @v
  * @oh: struct omap_hwmod *
@@ -749,7 +779,7 @@ static int _count_mpu_irqs(struct omap_hwmod *oh)
                ohii = &oh->mpu_irqs[i++];
        } while (ohii->irq != -1);
 
-       return i;
+       return i-1;
 }
 
 /**
@@ -772,7 +802,7 @@ static int _count_sdma_reqs(struct omap_hwmod *oh)
                ohdi = &oh->sdma_reqs[i++];
        } while (ohdi->dma_req != -1);
 
-       return i;
+       return i-1;
 }
 
 /**
@@ -795,7 +825,7 @@ static int _count_ocp_if_addr_spaces(struct omap_hwmod_ocp_if *os)
                mem = &os->addr[i++];
        } while (mem->pa_start != mem->pa_end);
 
-       return i;
+       return i-1;
 }
 
 /**
@@ -903,7 +933,9 @@ static void _enable_sysc(struct omap_hwmod *oh)
        }
 
        if (sf & SYSC_HAS_MIDLEMODE) {
-               if (oh->flags & HWMOD_SWSUP_MSTANDBY) {
+               if (oh->flags & HWMOD_FORCE_MSTANDBY) {
+                       idlemode = HWMOD_IDLEMODE_FORCE;
+               } else if (oh->flags & HWMOD_SWSUP_MSTANDBY) {
                        idlemode = HWMOD_IDLEMODE_NO;
                } else {
                        if (sf & SYSC_HAS_ENAWAKEUP)
@@ -970,7 +1002,8 @@ static void _idle_sysc(struct omap_hwmod *oh)
        }
 
        if (sf & SYSC_HAS_MIDLEMODE) {
-               if (oh->flags & HWMOD_SWSUP_MSTANDBY) {
+               if ((oh->flags & HWMOD_SWSUP_MSTANDBY) ||
+                   (oh->flags & HWMOD_FORCE_MSTANDBY)) {
                        idlemode = HWMOD_IDLEMODE_FORCE;
                } else {
                        if (sf & SYSC_HAS_ENAWAKEUP)
@@ -1366,6 +1399,12 @@ static int _ocp_softreset(struct omap_hwmod *oh)
        ret = _set_softreset(oh, &v);
        if (ret)
                goto dis_opt_clks;
+
+       _write_sysconfig(v, oh);
+       ret = _clear_softreset(oh, &v);
+       if (ret)
+               goto dis_opt_clks;
+
        _write_sysconfig(v, oh);
 
        if (oh->class->sysc->sysc_flags & SYSS_HAS_RESET_STATUS)
@@ -1852,6 +1891,11 @@ int omap_hwmod_softreset(struct omap_hwmod *oh)
                goto error;
        _write_sysconfig(v, oh);
 
+       ret = _clear_softreset(oh, &v);
+       if (ret)
+               goto error;
+       _write_sysconfig(v, oh);
+
 error:
        return ret;
 }