1 files changed, 38 insertions, 1 deletions

diff --git a/drivers/clk/rockchip/clk-mmc-phase.c b/drivers/clk/rockchip/clk-mmc-phase.c
index fe7d9ed1d436..dc4c227732bd 100644
--- a/drivers/clk/rockchip/clk-mmc-phase.c
+++ b/drivers/clk/rockchip/clk-mmc-phase.c
@@ -25,6 +25,8 @@ struct rockchip_mmc_clock {
         void __iomem    *reg;
         int             id;
         int             shift;
+        int             cached_phase;
+        struct notifier_block clk_rate_change_nb;
 };
 
 #define to_mmc_clock(_hw) container_of(_hw, struct rockchip_mmc_clock, hw)
@@ -162,6 +164,29 @@ static const struct clk_ops rockchip_mmc_clk_ops = {
         .set_phase      = rockchip_mmc_set_phase,
 };
 
+#define to_rockchip_mmc_clock(x) \
+        container_of(x, struct rockchip_mmc_clock, clk_rate_change_nb)
+static int rockchip_mmc_clk_rate_notify(struct notifier_block *nb,
+                                        unsigned long event, void *data)
+{
+        struct rockchip_mmc_clock *mmc_clock = to_rockchip_mmc_clock(nb);
+
+        /*
+         * rockchip_mmc_clk is mostly used by mmc controllers to sample
+         * the intput data, which expects the fixed phase after the tuning
+         * process. However if the clock rate is changed, the phase is stale
+         * and may break the data sampling. So here we try to restore the phase
+         * for that case.
+         */
+        if (event == PRE_RATE_CHANGE)
+                mmc_clock->cached_phase =
+                        rockchip_mmc_get_phase(&mmc_clock->hw);
+        else if (event == POST_RATE_CHANGE)
+                rockchip_mmc_set_phase(&mmc_clock->hw, mmc_clock->cached_phase);
+
+        return NOTIFY_DONE;
+}
+
 struct clk *rockchip_clk_register_mmc(const char *name,
                                 const char *const *parent_names, u8 num_parents,
                                 void __iomem *reg, int shift)
@@ -169,6 +194,7 @@ struct clk *rockchip_clk_register_mmc(const char *name,
         struct clk_init_data init;
         struct rockchip_mmc_clock *mmc_clock;
         struct clk *clk;
+        int ret;
 
         mmc_clock = kmalloc(sizeof(*mmc_clock), GFP_KERNEL);
         if (!mmc_clock)
@@ -186,7 +212,18 @@ struct clk *rockchip_clk_register_mmc(const char *name,
 
         clk = clk_register(NULL, &mmc_clock->hw);
         if (IS_ERR(clk))
-                kfree(mmc_clock);
+                goto err_register;
 
+        mmc_clock->clk_rate_change_nb.notifier_call =
+                                &rockchip_mmc_clk_rate_notify;
+        ret = clk_notifier_register(clk, &mmc_clock->clk_rate_change_nb);
+        if (ret)
+                goto err_notifier;
+
+        return clk;
+err_notifier:
+        clk_unregister(clk);
+err_register:
+        kfree(mmc_clock);
         return clk;
 }