1 files changed, 44 insertions, 1 deletions

diff --git a/arch/arm/mach-omap2/omap_hwmod.c b/arch/arm/mach-omap2/omap_hwmod.c
index ee655dab672f..8a1b5e0bad40 100644
--- a/arch/arm/mach-omap2/omap_hwmod.c
+++ b/arch/arm/mach-omap2/omap_hwmod.c
@@ -399,7 +399,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
  *
@@ -427,6 +427,36 @@ static int _set_softreset(struct omap_hwmod *oh, u32 *v)
 }
 
 /**
+ * _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;
+}
+
+/**
  * _wait_softreset_complete - wait for an OCP softreset to complete
  * @oh: struct omap_hwmod * to wait on
  *
@@ -785,6 +815,7 @@ static int _init_interface_clks(struct omap_hwmod *oh)
                         pr_warning("omap_hwmod: %s: cannot clk_get interface_clk %s\n",
                                    oh->name, os->clk);
                         ret = -EINVAL;
+                        continue;
                 }
                 os->_clk = c;
                 /*
@@ -821,6 +852,7 @@ static int _init_opt_clks(struct omap_hwmod *oh)
                         pr_warning("omap_hwmod: %s: cannot clk_get opt_clk %s\n",
                                    oh->name, oc->clk);
                         ret = -EINVAL;
+                        continue;
                 }
                 oc->_clk = c;
                 /*
@@ -1911,6 +1943,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->srst_udelay)
@@ -3227,6 +3265,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;
 }