ARM: tegra: dvfs: Re-factor DFLL output force control

Implemented output force register access functions, and re-factored
clients, respectively.

Updated debugfs force output control: enabled I2C PMIC control, fixed
order (swapped) of locking DFLL register access and enabling access
clock.

Change-Id: I33d656dfb384e97f55b46a0404578e6b0bc9b873
Signed-off-by: Alex Frid <afrid@nvidia.com>
Reviewed-on: http://git-master/r/383657
Reviewed-by: Yu-Huan Hsu <yhsu@nvidia.com>

diff --git a/arch/arm/mach-tegra/tegra_cl_dvfs.c b/arch/arm/mach-tegra/tegra_cl_dvfs.c
index a4235de36366d548bb86a90dec75d22ccbd4b452..761b4f1d78845847ab2f2059e51e1f803f49b661 100644 (file)
--- a/arch/arm/mach-tegra/tegra_cl_dvfs.c
+++ b/arch/arm/mach-tegra/tegra_cl_dvfs.c
@@ -324,7 +324,23 @@ static inline void invalidate_request(struct tegra_cl_dvfs *cld)
        cl_dvfs_wmb(cld);
 }
 
-static inline void disable_forced_output(struct tegra_cl_dvfs *cld)
+static inline u32 output_force_set_val(struct tegra_cl_dvfs *cld, u8 out_val)
+{
+       u32 val = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
+       val = (val & CL_DVFS_OUTPUT_FORCE_ENABLE) | (out_val & OUT_MASK);
+       cl_dvfs_writel(cld, val, CL_DVFS_OUTPUT_FORCE);
+       return cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
+}
+
+static inline void output_force_enable(struct tegra_cl_dvfs *cld)
+{
+       u32 val = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
+       val |= CL_DVFS_OUTPUT_FORCE_ENABLE;
+       cl_dvfs_writel(cld, val, CL_DVFS_OUTPUT_FORCE);
+       cl_dvfs_wmb(cld);
+}
+
+static inline void output_force_disable(struct tegra_cl_dvfs *cld)
 {
        u32 val = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
        val &= ~CL_DVFS_OUTPUT_FORCE_ENABLE;
@@ -897,7 +913,7 @@ static void cl_dvfs_calibrate(struct tegra_cl_dvfs *cld)
                /* Forced output must be disabled in closed loop mode */
                val = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
                if (val & CL_DVFS_OUTPUT_FORCE_ENABLE) {
-                       disable_forced_output(cld);
+                       output_force_disable(cld);
                        calibration_timer_update(cld);
                        return;
                }
@@ -1789,16 +1805,10 @@ static int tegra_cl_dvfs_force_output(void *data, unsigned int out_sel)
 
        clk_lock_save(cld->dfll_clk, &flags);
 
-       val = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
-       val = (val & CL_DVFS_OUTPUT_FORCE_ENABLE) | out_sel;
-       cl_dvfs_writel(cld, val, CL_DVFS_OUTPUT_FORCE);
-       val = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
-
+       val = output_force_set_val(cld, out_sel);
        if ((cld->mode < TEGRA_CL_DVFS_CLOSED_LOOP) &&
            !(val & CL_DVFS_OUTPUT_FORCE_ENABLE)) {
-               val |= CL_DVFS_OUTPUT_FORCE_ENABLE;
-               cl_dvfs_writel(cld, val, CL_DVFS_OUTPUT_FORCE);
-               cl_dvfs_wmb(cld);
+               output_force_enable(cld);
                /* enable output only if bypass h/w is alive */
                if (!cld->safe_dvfs->dfll_data.is_bypass_down ||
                    !cld->safe_dvfs->dfll_data.is_bypass_down())
@@ -2544,7 +2554,7 @@ int tegra_cl_dvfs_lock(struct tegra_cl_dvfs *cld)
                output_enable(cld);
                set_mode(cld, TEGRA_CL_DVFS_CLOSED_LOOP);
                set_request(cld, req);
-               disable_forced_output(cld);
+               output_force_disable(cld);
                calibration_timer_update(cld);
                return 0;
 
@@ -2822,33 +2832,19 @@ static int fout_mv_set(void *data, u64 val)
        struct clk *c = (struct clk *)data;
        struct tegra_cl_dvfs *cld = c->u.dfll.cl_dvfs;
 
-       /* FIXME: do we need it in i2c mode ? */
-       if (is_i2c(cld))
-               return -ENOSYS;
-
-       clk_lock_save(c, &flags);
        clk_enable(cld->soc_clk);
+       clk_lock_save(c, &flags);
 
-       v = cl_dvfs_readl(cld, CL_DVFS_OUTPUT_FORCE);
        if (val) {
-               val = find_mv_out_cap(cld, (int)val);
-               v = (v & CL_DVFS_OUTPUT_FORCE_ENABLE) | (u32)val;
-               cl_dvfs_writel(cld, v, CL_DVFS_OUTPUT_FORCE);
-               cl_dvfs_wmb(cld);
-
-               if (!(v & CL_DVFS_OUTPUT_FORCE_ENABLE)) {
-                       v |= CL_DVFS_OUTPUT_FORCE_ENABLE;
-                       cl_dvfs_writel(cld, v, CL_DVFS_OUTPUT_FORCE);
-                       cl_dvfs_wmb(cld);
-               }
-       } else if (v & CL_DVFS_OUTPUT_FORCE_ENABLE) {
-               v &= ~CL_DVFS_OUTPUT_FORCE_ENABLE;
-               cl_dvfs_writel(cld, v, CL_DVFS_OUTPUT_FORCE);
-               cl_dvfs_wmb(cld);
+               v = output_force_set_val(cld, find_mv_out_cap(cld, (int)val));
+               if (!(v & CL_DVFS_OUTPUT_FORCE_ENABLE))
+                       output_force_enable(cld);
+       } else {
+               output_force_disable(cld);
        }
 
-       clk_disable(cld->soc_clk);
        clk_unlock_restore(c, &flags);
+       clk_disable(cld->soc_clk);
        return 0;
 }
 DEFINE_SIMPLE_ATTRIBUTE(fout_mv_fops, fout_mv_get, fout_mv_set, "%llu\n");